Add example 10 (#112)

Author: pockerman

examples/example_10/CMakeLists.txt

@@ -0,0 +1,9 @@
+CMAKE_MINIMUM_REQUIRED(VERSION 3.20)
+
+SET(EXECUTABLE  example_10)
+SET(SOURCE ${EXECUTABLE}.cpp)
+
+ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
+
+TARGET_LINK_LIBRARIES(${EXECUTABLE} rlenvscpplib)
+

examples/example_10/example_10.cpp

@@ -0,0 +1,152 @@
+/*
+ * In this example we simulate a quadrotor.
+ * The quadrotor data is taken from 
+ * https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf
+ * 
+ * 
+ * 
+ */ 
+
+#include "rlenvs/rlenvs_types_v2.h"
+#include "rlenvs/dynamics/quadrotor_dynamics.h"
+#include "rlenvs/utils/io/csv_file_writer.h"
+#include "rlenvs/utils/unit_converter.h"
+
+#include <iostream>
+#include <string>
+#include <map>
+#include <any>
+#include <array>
+#include <vector>
+
+namespace example_10{
+	
+	using namespace rlenvscpp::dynamics;
+	using rlenvscpp::uint_t;
+	using rlenvscpp::real_t;
+	using rlenvscpp::RealVec;
+	using rlenvscpp::utils::io::CSVWriter;
+	
+	real_t compute_motor_speed(real_t t){
+		
+		if(t <= 0.1){
+			return 500 * t + 625.0;
+		}
+		else if( t > 0.1 && t <= 0.4){
+			return -416.66 * t + 716.66;
+		}
+		else if( t > 0.4 && t <= 0.5){
+			return 750.0 * t + 250.0;
+		}
+		else{
+			return 625.0;
+		}
+		
+	}
+	
+}
+
+
+int main(){
+
+	using namespace example_10;
+	
+	QuadrotorDynamicsConfig config;
+	config.dt = 0.0001;
+	config.mass = 0.468;
+	config.l = 0.225;
+	config.k_1 = 2.980e-6; // this is the k coeff
+	config.k_2 = 1.140e-7; // this is the beta coeff
+	config.Jx = 4.856e-3;
+	config.Jy = 4.856e-3;
+	config.Jz = 8.801e-3;
+	
+	
+	// initialize the state variables
+	std::array<std::pair<std::string, real_t>, 12> values;
+	
+	// initial position in NED
+	values[0] = std::make_pair("x", 0.0);
+	values[1] = std::make_pair("y", 0.0);
+	values[2] = std::make_pair("z", 0.0);
+	
+	// initial translation velocities body coords
+	values[3] = std::make_pair("u", 0.0);
+	values[4] = std::make_pair("v", 0.0);
+	values[5] = std::make_pair("w", 0.0);
+	
+	// initial rotational velocities body coords
+	values[6] = std::make_pair("p", 0.0);
+	values[7] = std::make_pair("q", 0.0);
+	values[8] = std::make_pair("r", 0.0);
+	
+	// Euler angles
+	values[9]  = std::make_pair("phi", 0.0);
+	values[10] = std::make_pair("theta", 0.0);
+	values[11] = std::make_pair("psi", 0.0);
+	
+	
+	SysState<12> state(std::move(values));
+	QuadrotorDynamics dynamics(config, state);
+	
+	// the motor velocities rad/sec
+	RealVec omegas = RealVec::Zero(4);
+	omegas[0] = omegas[1] = omegas[2] = omegas[3] = 625.0;
+	
+	
+	CSVWriter csv_writer("state");
+	csv_writer.open();
+	
+	std::vector<std::string> names={"t", 
+									"x", "y", "z",
+	                                "u", "v", "w",
+									"p", "q", "r",
+									"phi", "theta", "psi"};
+
+	csv_writer.write_column_names(names);								
+	
+	const real_t T = 2.0;
+	real_t time = 0.0;
+	
+	std::vector<real_t> row(13, 0.0);
+	while(time < T){
+		
+		std::cout<<"Time: "<<time<<std::endl;
+		
+		auto omega_motor = compute_motor_speed(time);
+		omegas[0] = omegas[1] = omegas[2] = omegas[3] = omega_motor;
+		dynamics.integrate(omegas);
+		
+		auto p = dynamics.get_position();
+		auto v = dynamics.get_velocity();
+		auto omega = dynamics.get_angular_velocity();
+		auto euler = dynamics.get_euler_angles();
+		
+		row[0] = time;
+		row[1] = p[0];
+		row[2] = p[1];
+		row[3] = p[2];
+		
+		row[4] = v[0];
+		row[5] = v[1];
+		row[6] = v[2];
+		
+		row[7] = omega[0];
+		row[8] = omega[1];
+		row[9] = omega[2];
+		
+		row[10] = rlenvscpp::utils::unit_converter::rad_to_degrees(euler[0]);
+		row[11] = rlenvscpp::utils::unit_converter::rad_to_degrees(euler[1]);
+		row[12] = rlenvscpp::utils::unit_converter::rad_to_degrees(euler[2]);
+		
+		csv_writer.write_row(row);
+		std::cout<<"Current position: ";
+		std::cout<<p<<std::endl;
+		std::cout<<euler<<std::endl;
+		time += config.dt;
+	}
+	
+	csv_writer.close();
+	
+    return 0;
+}

examples/example_10/plot.py

@@ -0,0 +1,85 @@
+"""
+Utility script for plotting with matplotlib
+"""
+import matplotlib.pyplot as plt
+import csv
+import math
+import numpy as np
+
+def main(filename):
+	
+	with open(filename, 'r', newline='') as csvfile:
+		csv_file_reader = csv.reader(csvfile, delimiter=",")
+		
+		time = []
+		x = []
+		y = []
+		z = []
+		u = []
+		v = []
+		w = []
+		p = []
+		q = []
+		r = []
+		phi = []
+		theta = []
+		psi = []
+		
+		for row in csv_file_reader:
+			if not row[0].startswith('#'):
+				try:
+
+					assert len(row) == 13
+
+					time.append(float(row[0]))
+					x.append(float(row[1]))
+					y.append(float(row[2]))
+					z.append(float(row[3]))
+					
+					u.append(float(row[4]))
+					v.append(float(row[5]))
+					w.append(float(row[6]))
+					
+					p.append(float(row[7]))
+					q.append(float(row[8]))
+					r.append(float(row[9]))
+
+					phi.append(float(row[10]))
+					theta.append(float(row[11]))
+					psi.append(float(row[12]))
+				except Exception as e:
+					print(e)
+					continue
+		
+		assert len(time) == len(x), "Invalid length"
+		plt.plot(time, x, label='X^G', linewidth=2)
+		plt.plot(time, y, label='Y^G', linewidth=2)
+		plt.plot(time, z, label='Z^G', linewidth=2)
+		
+		plt.xlabel('time (secs)')
+		plt.ylabel('position')
+		plt.title('Quadrotor position in NED')
+		plt.grid()
+		plt.legend()
+		plt.show()
+
+
+		assert len(time) == len(phi), "Invalid length"
+
+		plt.plot(time, phi, label='$\phi$', linewidth=2)
+		plt.plot(time, theta, label='$\theta$', linewidth=2)
+		plt.plot(time, psi, label='$\psi$', linewidth=2)
+		plt.xlabel('time (secs)')
+		plt.ylabel('Angle')
+		plt.title('Euler angles (deg)')
+		plt.grid()
+		plt.legend()
+		plt.show()
+		
+		
+if __name__ == '__main__':
+  main("/home/alex/qi3/rlenvs_from_cpp/build/examples/example_10/state.csv")
+	
+	
+
+