Merge pull request #118 from pockerman/feature_112_add_dynamics_library

Feature 112 add dynamics library

Author: pockerman

CMakeLists.txt

@@ -1,4 +1,4 @@
-CMAKE_MINIMUM_REQUIRED(VERSION 3.6)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.20)
 MESSAGE(STATUS "Using CMake ${CMAKE_VERSION}")
 
 SET(RLENVSCPP_VERSION_MAJOR 1)
@@ -103,6 +103,7 @@ FILE(GLOB SRCS src/rlenvs/*.cpp
 			   src/rlenvs/envs/connect2/*.cpp
 			   src/rlenvs/dynamics/*.cpp
 			   src/rlenvs/utils/*.cpp
+			   src/rlenvs/utils/io/*.cpp
    )
 
 ADD_LIBRARY(rlenvscpplib SHARED ${SRCS})

README.md

@@ -19,7 +19,7 @@ Currently, we provide a minimal number of wrappers for some common Gymnasium (fo
 In addition there are wrappers for
 
 - ```GymWalk``` environment from <a href="https://github.com/mimoralea/gym-walk2">gym_walk</a>
-- ```gym-pybullet-drones``` from <a href="https://github.com/utiasDSL/gym-pybullet-drones/tree/main">gym-pybullet-drones</a>
+- ~```gym-pybullet-drones``` from <a href="https://github.com/utiasDSL/gym-pybullet-drones/tree/main">gym-pybullet-drones</a>~
 - ```GridWorld``` from <a href="https://github.com/DeepReinforcementLearning/DeepReinforcementLearningInAction">Deep Reinforcement Learning In Action</a>
 - ```Connect2``` from <a href="https://github.com/JoshVarty/AlphaZeroSimple">AlphaZeroSimple</a> see <a href="examples/example_7/example_7.cpp">example_7</a>
 
@@ -33,7 +33,8 @@ There exist some wrappers for vector environments:
 
 Apart from the exposed environments, ```rlenvscpp``` exposes classes that describe the dynamics of some popular rigid bodies:
 
-- <a href="https://en.wikipedia.org/wiki/Differential_wheeled_robot">Differential drive</a> see <a href="examples/example_9/example_9.cpp">example_9</a>
+- <a href="https://en.wikipedia.org/wiki/Differential_wheeled_robot">Differential drive dynamics</a> see <a href="examples/example_9/example_9.cpp">example_9</a>
+- <a href="https://scholarsarchive.byu.edu/cgi/viewcontent.cgi?article=2324&context=facpub">Quadrotor dynamics</a> see <a href="examples/example_10/example_10.cpp">example_10</a>
 
 In general, the environments exposed by the library  should abide with <a href="https://github.com/deepmind/dm_env/blob/master/docs/index.md">dm_env</a> specification.
 The following snippet shows how to use the ```FrozenLake```  and ```Taxi``` environments from <a href="https://github.com/Farama-Foundation/Gymnasium/tree/main">Gymnasium</a>.
@@ -220,7 +221,6 @@ In addition, you need to install
 In addition, the library also incorporates, see ```(src/extern)```, the following libraries
 
 - <a href="https://github.com/elnormous/HTTPRequest">HTTPRequest</a>
-- <a href="http://github.com/aantron/better-enums">better-enums</a>
 - <a href="https://github.com/nlohmann/json">nlohmann/json</a>
 
 There are extra dependencies if you want to generate the documentation. Namely,

examples/CMakeLists.txt

@@ -1,10 +1,10 @@
-CMAKE_MINIMUM_REQUIRED(VERSION 3.6)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.20)
 
 INCLUDE_DIRECTORIES(${PROJECT_INCL_DIR}) 
 INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})
 
-SET(CMAKE_CXX_COMPILER /usr/bin/g++-11)
-SET(CMAKE_C_COMPILER /usr/bin/gcc-11)
+#SET(CMAKE_CXX_COMPILER /usr/bin/g++-11)
+#SET(CMAKE_C_COMPILER /usr/bin/gcc-11)
 SET(CMAKE_CXX_STANDARD 20)
 SET(CMAKE_CXX_STANDARD_REQUIRED True) 
 SET(CMAKE_LINKER_FLAGS "-pthread")
@@ -29,3 +29,4 @@ ADD_SUBDIRECTORY(example_6)
 ADD_SUBDIRECTORY(example_7)
 ADD_SUBDIRECTORY(example_8)
 ADD_SUBDIRECTORY(example_9)
+ADD_SUBDIRECTORY(example_10)

examples/example_1/CMakeLists.txt

@@ -1,4 +1,4 @@
-CMAKE_MINIMUM_REQUIRED(VERSION 3.6)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.20)
 
 SET(EXECUTABLE  example_1)
 SET(SOURCE ${EXECUTABLE}.cpp)

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")
+	
+	
+
+

examples/example_2/CMakeLists.txt

@@ -1,4 +1,4 @@
-CMAKE_MINIMUM_REQUIRED(VERSION 3.6)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.20)
 
 SET(SOURCE example_2.cpp)
 SET(EXECUTABLE  example_2)

examples/example_3/CMakeLists.txt

@@ -1,4 +1,4 @@
-CMAKE_MINIMUM_REQUIRED(VERSION 3.6)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.20)
 
 SET(SOURCE example_3.cpp)
 SET(EXECUTABLE  example_3)

examples/example_4/CMakeLists.txt

@@ -1,4 +1,4 @@
-CMAKE_MINIMUM_REQUIRED(VERSION 3.6)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.20)
 
 SET(SOURCE example_4.cpp)
 SET(EXECUTABLE  example_4)