├── demo.gif ├── flappy ├── urdf │ ├── fwmav │ │ ├── meshes │ │ │ ├── left_LE.STL │ │ │ ├── torso.STL │ │ │ ├── left_wing.STL │ │ │ ├── right_LE.STL │ │ │ ├── right_wing.STL │ │ │ ├── torso_no_base.STL │ │ │ ├── torso_small_base.STL │ │ │ └── Wing_Camber_70mm_C35mm_no_trailing_L.STL │ │ └── flapper_sc_nominal.urdf │ └── ground.urdf ├── envs │ ├── Wing.i │ ├── fwmav │ │ ├── config │ │ │ ├── sim_config.json │ │ │ └── mav_config_list.json │ │ ├── __init__.py │ │ ├── mission.py │ │ ├── policy_post_processing.py │ │ ├── Actuator.hpp │ │ ├── fwmav_sim_env_simple.py │ │ ├── MyGLUTWindow.py │ │ ├── Actuator.cpp │ │ ├── ddpg_fwmav.py │ │ ├── sensor_for_no_pos.py │ │ ├── sensor.py │ │ ├── fwmav_sim_en_old_with_control_working.py │ │ ├── fwmav_sim_env.py │ │ ├── sensor_fusion.py │ │ ├── fwmav_maneuver_env.py │ │ ├── sensor_fusion_pos.py │ │ └── controllers │ │ │ ├── pid_controller.py │ │ │ ├── controller_no_base.py │ │ │ └── controller_maneuver.py │ ├── Wing.hpp │ ├── Wing.py │ └── Wing.cpp └── __init__.py ├── setup.py ├── LICENSE ├── train.py ├── test.py ├── train_DDPG.py ├── train_maneuver_DDPG.py ├── test_simple.py ├── fwmav_sim_env_maneuver.py ├── README.md └── fwmav_sim_env.py /demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/purdue-biorobotics/flappy/HEAD/demo.gif -------------------------------------------------------------------------------- /flappy/urdf/fwmav/meshes/left_LE.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/purdue-biorobotics/flappy/HEAD/flappy/urdf/fwmav/meshes/left_LE.STL -------------------------------------------------------------------------------- /flappy/urdf/fwmav/meshes/torso.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/purdue-biorobotics/flappy/HEAD/flappy/urdf/fwmav/meshes/torso.STL -------------------------------------------------------------------------------- /flappy/envs/Wing.i: -------------------------------------------------------------------------------- 1 | %module Wing 2 | 3 | %{ 4 | #define SWIG_FILE_WITH_INIT 5 | #include "Wing.hpp" 6 | %} 7 | 8 | %include "Wing.hpp" -------------------------------------------------------------------------------- /flappy/urdf/fwmav/meshes/left_wing.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/purdue-biorobotics/flappy/HEAD/flappy/urdf/fwmav/meshes/left_wing.STL -------------------------------------------------------------------------------- /flappy/urdf/fwmav/meshes/right_LE.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/purdue-biorobotics/flappy/HEAD/flappy/urdf/fwmav/meshes/right_LE.STL -------------------------------------------------------------------------------- /flappy/urdf/fwmav/meshes/right_wing.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/purdue-biorobotics/flappy/HEAD/flappy/urdf/fwmav/meshes/right_wing.STL -------------------------------------------------------------------------------- /flappy/urdf/fwmav/meshes/torso_no_base.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/purdue-biorobotics/flappy/HEAD/flappy/urdf/fwmav/meshes/torso_no_base.STL -------------------------------------------------------------------------------- /flappy/urdf/fwmav/meshes/torso_small_base.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/purdue-biorobotics/flappy/HEAD/flappy/urdf/fwmav/meshes/torso_small_base.STL -------------------------------------------------------------------------------- /flappy/urdf/fwmav/meshes/Wing_Camber_70mm_C35mm_no_trailing_L.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/purdue-biorobotics/flappy/HEAD/flappy/urdf/fwmav/meshes/Wing_Camber_70mm_C35mm_no_trailing_L.STL -------------------------------------------------------------------------------- /flappy/envs/fwmav/config/sim_config.json: -------------------------------------------------------------------------------- 1 | { 2 | "f_sim" : 1e4, 3 | "f_driver" : 1e3, 4 | "f_sensor" : 500, 5 | "f_control" : 500, 6 | "f_imu" : 1000, 7 | "f_vicon" : 200, 8 | "f_visual" : 24 9 | } -------------------------------------------------------------------------------- /flappy/envs/fwmav/__init__.py: -------------------------------------------------------------------------------- 1 | from flappy.envs.fwmav.fwmav_sim_env import FWMAVSimEnv 2 | from flappy.envs.fwmav.fwmav_sim_env_simple import FWMAVSimEnvSimple 3 | from flappy.envs.fwmav.fwmav_maneuver_env import FWMAVManeuverEnv 4 | -------------------------------------------------------------------------------- /flappy/__init__.py: -------------------------------------------------------------------------------- 1 | from gym.envs.registration import register 2 | 3 | register( 4 | id='fwmav_hover-v0', 5 | entry_point='flappy.envs.fwmav:FWMAVSimEnv', 6 | ) 7 | 8 | register( 9 | id='fwmav_hover-v1', 10 | entry_point='flappy.envs.fwmav:FWMAVSimEnvSimple', 11 | ) 12 | 13 | register( 14 | id='fwmav_maneuver-v0', 15 | entry_point='flappy.envs.fwmav:FWMAVManeuverEnv', 16 | ) 17 | -------------------------------------------------------------------------------- /flappy/envs/fwmav/mission.py: -------------------------------------------------------------------------------- 1 | ########################## FWMAV Simulation ######################### 2 | # Version 0.2 3 | # Fan Fei Mar 2018 4 | # Quadrotor simulation with simplified aerodynamics 5 | ####################################################################### 6 | 7 | import numpy as np 8 | 9 | class Mission: 10 | def __init__(self): 11 | self.pos_target_x_ = 0 12 | self.pos_target_y_ = 0 13 | self.pos_target_z_ = 0 14 | 15 | def reset(self): 16 | self.pos_target_x_ = 0 17 | self.pos_target_y_ = 0 18 | self.pos_target_z_ = 0 19 | 20 | def update_waypoint(self,time): 21 | # update mission here 22 | self.pos_target_x_ = 0 23 | self.pos_target_y_ = 0 24 | self.pos_target_z_ = 0 -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | ########################## FWMAV Simulation ######################### 2 | # Version 0.3 3 | # Fan Fei Feb 2019 4 | # Direct motor driven flapping wing MAV simulation 5 | ####################################################################### 6 | 7 | from setuptools import setup 8 | from distutils.core import Extension 9 | from sys import platform as _platform 10 | 11 | CXX_FLAGS = '' 12 | if _platform == "darwin": 13 | CXX_FLAGS += '-mmacosx-version-min=10.9 ' 14 | 15 | Wing_module = Extension('_Wing', 16 | sources = ['flappy/envs/Wing_wrap.cxx', 'flappy/envs/Wing.cpp'], 17 | extra_compile_args=CXX_FLAGS.split()) 18 | 19 | setup (name = 'Wing', 20 | version = '0.1', 21 | author = "Fan Fei", 22 | description = "Flapping wing with aero model", 23 | install_requires=['gym', 'stable_baselines', 'numpy'], 24 | ext_modules = [Wing_module], 25 | py_modules = ["Wing"]) -------------------------------------------------------------------------------- /flappy/urdf/ground.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the ""Software""), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 4 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 5 | THE SOFTWARE IS PROVIDED *AS IS*, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 6 | 7 | -------------------------------------------------------------------------------- /flappy/envs/fwmav/policy_post_processing.py: -------------------------------------------------------------------------------- 1 | import pickle 2 | import os 3 | import numpy as np 4 | import theano 5 | import lasagne 6 | 7 | log_dir = os.path.join(os.getcwd(),'data') 8 | 9 | with open(os.path.join(log_dir,'policy.pkl'), 'rb') as input: 10 | policy = pickle.load(input) 11 | 12 | h0w = policy._cached_params[()][0].get_value() 13 | h0b = policy._cached_params[()][1].get_value() 14 | h1w = policy._cached_params[()][2].get_value() 15 | h1b = policy._cached_params[()][3].get_value() 16 | ow = policy._cached_params[()][4].get_value() 17 | ob = policy._cached_params[()][5].get_value() 18 | 19 | ologstd = policy._cached_params[()][6].get_value() 20 | 21 | obser = np.linspace(0,17,18).reshape(18,1) 22 | 23 | h0_out = np.tanh(np.matmul(obser.T, h0w) + h0b) 24 | h1_out = np.tanh(np.matmul(h0_out, h1w) + h1b) 25 | out = np.matmul(h1_out, ow) + ob 26 | 27 | # for testing 28 | policy.get_action(obser) 29 | # same as 30 | l_out = policy._l_mean 31 | l_in = policy._mean_network.input_layer 32 | f = theano.function([l_in.input_var], lasagne.layers.get_output(l_out)) 33 | print(out) 34 | print(f(obser.T)) -------------------------------------------------------------------------------- /flappy/envs/fwmav/Actuator.hpp: -------------------------------------------------------------------------------- 1 | /************************* FWMAV Simulation ************************* 2 | * Version 0.3 3 | * Fan Fei Jan 2018 4 | * FWMAV simulation with dual motor driven robotic flapper 5 | * PID controller using split cycle mechanism 6 | *********************************************************************** 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | namespace FWMAV{ 14 | class Actuator { 15 | public: 16 | Actuator(double variable_resistance, double* stroke_velocity, double* stroke_acceleration); 17 | virtual ~Actuator(); 18 | 19 | void doNothing(); 20 | void updateDriverVoltage(double voltage); 21 | void UpdateTorque(); 22 | 23 | double GetTorque(); 24 | double GetMotorTorque(); 25 | double GetMagTorque(); 26 | double GetInerTorque(); 27 | double GetDampTorque(); 28 | double GetFricTorque(); 29 | double GetBEMF(); 30 | double GetCurrent(); 31 | 32 | protected: 33 | double* stroke_velocity_; 34 | double* stroke_acceleration_; 35 | 36 | const double k_resistance_; 37 | const double k_torque_constant_; 38 | const double k_gear_ratio_; 39 | const double k_mechanical_efficiency_; 40 | const double k_friction_coefficient_; 41 | const double k_damping_coefficient_; 42 | const double k_inertia_; 43 | 44 | 45 | double psi_dot_; 46 | double psi_ddot_; 47 | double motor_vel_; 48 | double motor_accel_; 49 | double sign_; 50 | double voltage_; 51 | double current_; 52 | double back_EMF_; 53 | double variable_resistance_; 54 | double resistance_; 55 | 56 | double inertia_torque_; 57 | double damping_torque_; 58 | double friction_torque_; 59 | double magnetic_torque_; 60 | double motor_torque_; 61 | double output_torque_; 62 | }; 63 | } -------------------------------------------------------------------------------- /flappy/envs/fwmav/fwmav_sim_env_simple.py: -------------------------------------------------------------------------------- 1 | ########################## FWMAV Simulation ######################### 2 | # Version 0.3 3 | # Fan Fei Feb 2019 4 | # Direct motor driven flapping wing MAV simulation 5 | ####################################################################### 6 | 7 | import numpy as np 8 | 9 | from flappy.envs.fwmav.fwmav_sim_env import FWMAVSimEnv 10 | import time 11 | 12 | class FWMAVSimEnvSimple(FWMAVSimEnv): 13 | def __init__(self): 14 | super().__init__() 15 | self.action_lb = np.array([-18.0, -18.0]) 16 | self.action_ub = np.array([18.0, 18.0]) 17 | self.total_action_lb = np.array([-18.0, -18.0]) 18 | self.total_action_ub = np.array([18.0, 18.0]) 19 | self.action_old = np.array([0, 0]) 20 | 21 | self.random_init = False 22 | 23 | 24 | def step(self, action): 25 | # scale action from [-1,1] to [action_lb, action_ub] 26 | # since baseline does not support asymmetric action space 27 | scaled_action = (action+1)*0.5*(self.action_ub-self.action_lb)+self.action_lb 28 | scaled_action = np.clip(scaled_action, self.action_lb, self.action_ub) 29 | 30 | input_voltage = np.clip(scaled_action, self.total_action_lb, self.total_action_ub) 31 | 32 | # run simulation and down sample from f_sim to f_control 33 | while self.sim.world.time() < self.next_control_time: 34 | self.sim.step(input_voltage) 35 | self.next_control_time += self.sim.dt_c 36 | 37 | # update control target 38 | self.mission.update_waypoint(self.sim.world.time()) 39 | 40 | # update observation, reward, terminal 41 | self.observation = self.get_observation() 42 | reward = self.get_reward(action) 43 | done = self.get_terminal() 44 | 45 | # visulization 46 | if self.is_visual_on and time.time() >= self.next_visualization_time: 47 | self.gui.cv.wait() 48 | self.next_visualization_time = time.time() + self.dt_v 49 | 50 | info = {'time' : self.sim.world.time()} 51 | return self.observation, reward, done, info 52 | -------------------------------------------------------------------------------- /flappy/envs/fwmav/MyGLUTWindow.py: -------------------------------------------------------------------------------- 1 | ########################## FWMAV Simulation ######################### 2 | # Version 0.3 3 | # Ruoyu Wu, Fan Fei Feb 2019 4 | # Direct motor driven flapping wing MAV simulation 5 | ####################################################################### 6 | 7 | import pydart2 as pydart 8 | from pydart2.gui.glut.window import GLUTWindow 9 | from pydart2.gui.trackball import Trackball 10 | 11 | from threading import Thread, Condition 12 | import OpenGL.GLUT as GLUT 13 | import sys 14 | 15 | 16 | class MyGLUTWindow(GLUTWindow): 17 | def __init__(self, world, sim, env, title, cv): 18 | self.sim_ob = sim 19 | self.env = env 20 | self.cv = cv 21 | super().__init__(world, title) 22 | self.window_size = (800, 600) 23 | 24 | def keyPressed(self, key, x, y): 25 | keycode = ord(key) 26 | key = key.decode('utf-8') 27 | 28 | if keycode == 27: 29 | GLUT.glutDestroyWindow(self.window) 30 | elif key == 'q': 31 | GLUT.glutDestroyWindow(self.window) 32 | elif key == 'r': 33 | self.env.reset() 34 | elif key == ' ': 35 | self.env.is_sim_on = not self.env.is_sim_on 36 | 37 | 38 | def idle(self, ): 39 | pass 40 | 41 | def drawGL(self, ): 42 | self.cv.acquire() 43 | 44 | # rendering 45 | self.scene.render(self.sim) 46 | GLUT.glutSwapBuffers() 47 | 48 | self.cv.notify() 49 | self.cv.release() 50 | 51 | class GUI(Thread): 52 | def __init__(self, env, title): 53 | Thread.__init__(self) 54 | self.cv = Condition() 55 | self.title = title 56 | self.env = env 57 | self.sim = env.sim 58 | self.win = MyGLUTWindow(self.sim.world, self.sim, self.env, self.title, self.cv) 59 | 60 | self.camera_theta = 75#, for mirror 85 61 | self.camera_phi = 135#, for mirror -75 62 | self.camera_horizontal = 0.0 63 | self.camera_vertical = -0.25 64 | self.camera_depth = -1.25 65 | self.camera_0 = Trackball(theta = self.camera_theta, phi = self.camera_phi, trans=[self.camera_horizontal, self.camera_vertical, self.camera_depth]) 66 | self.win.scene.replace_camera(0,self.camera_0) 67 | 68 | def run(self): 69 | pydart.gui.viewer.launch_window(self.sim.world, self.win, 0) 70 | 71 | 72 | -------------------------------------------------------------------------------- /train.py: -------------------------------------------------------------------------------- 1 | ########################## FWMAV Simulation ######################### 2 | # Version 0.3 3 | # Fan Fei Feb 2019 4 | # Direct motor driven flapping wing MAV simulation 5 | ####################################################################### 6 | 7 | import gym 8 | import flappy 9 | 10 | from stable_baselines.common.policies import MlpPolicy 11 | from stable_baselines.common.vec_env import DummyVecEnv 12 | from stable_baselines.common.vec_env import SubprocVecEnv 13 | from stable_baselines.common import set_global_seeds 14 | 15 | import time 16 | import argparse 17 | import importlib 18 | 19 | 20 | def make_env(env_id, rank, seed=0, random_init = True, randomize_sim = True, phantom_sensor = False): 21 | def _init(): 22 | env = gym.make(env_id) 23 | env.config(random_init, randomize_sim, phantom_sensor) 24 | if rank == 0: 25 | env.enable_visualization() 26 | env.enable_print() 27 | env.seed(seed + rank) 28 | return env 29 | 30 | # set_global_seeds(seed) 31 | return _init 32 | 33 | def main(args): 34 | 35 | try: 36 | model_cls = getattr(importlib.import_module( 37 | 'stable_baselines'), args.model_type) 38 | except AttributeError: 39 | print(args.model_type, "Error: wrong model type") 40 | return 41 | 42 | try: 43 | policy_cls = getattr(importlib.import_module( 44 | 'stable_baselines.common.policies'), args.policy_type) 45 | except AttributeError: 46 | print(args.policy_type, "Error: wrong policy type") 47 | return 48 | 49 | start = time.time() 50 | 51 | env_id = 'fwmav_hover-v0' 52 | # env = DummyVecEnv([make_env(env_id, 1)]) 53 | env = SubprocVecEnv([make_env(env_id, i) for i in range(args.n_cpu)]) 54 | 55 | model = model_cls(policy_cls, env, verbose=0) 56 | model.learn(total_timesteps=args.time_step) 57 | model.save(args.model_path) 58 | 59 | end = time.time() 60 | print("Time used: ", end - start) 61 | 62 | 63 | if __name__ == '__main__': 64 | parser = argparse.ArgumentParser() 65 | parser.add_argument('--model_type', const='PPO2', default='PPO2', nargs='?') 66 | parser.add_argument('--model_path', required=True, nargs='?') 67 | parser.add_argument('--policy_type', 68 | const='MlpPolicy', default='MlpPolicy', nargs='?') 69 | parser.add_argument('--n_cpu', const=4, default=4, type=int, nargs='?') 70 | parser.add_argument('--time_step', required=True, type=int, nargs='?') 71 | args = parser.parse_args() 72 | 73 | main(args) 74 | -------------------------------------------------------------------------------- /flappy/envs/fwmav/Actuator.cpp: -------------------------------------------------------------------------------- 1 | /************************* FWMAV Simulation ************************* 2 | * Version 0.3 3 | * Fan Fei Jan 2018 4 | * FWMAV simulation with dual motor driven robotic flapper 5 | * PID controller using split cycle mechanism 6 | *********************************************************************** 7 | */ 8 | 9 | #include "Actuator.hpp" 10 | 11 | FWMAV::Actuator::Actuator(double variable_resistance, double* stroke_velocity, double* stroke_acceleration): 12 | k_resistance_(12.4), 13 | k_torque_constant_(1.75E-3), 14 | k_gear_ratio_(10), 15 | k_mechanical_efficiency_(0.85), 16 | k_friction_coefficient_(2E-5), 17 | k_damping_coefficient_(9.74E-9), 18 | k_inertia_(7.03e-10) 19 | { 20 | variable_resistance_ = variable_resistance; 21 | stroke_velocity_ = stroke_velocity; 22 | stroke_acceleration_ = stroke_acceleration; 23 | doNothing(); 24 | } 25 | 26 | 27 | FWMAV::Actuator::~Actuator() 28 | { 29 | 30 | } 31 | 32 | void FWMAV::Actuator::doNothing() 33 | { 34 | output_torque_ = 0; 35 | } 36 | 37 | void FWMAV::Actuator::updateDriverVoltage(double voltage) 38 | { 39 | voltage_ = voltage; 40 | } 41 | 42 | void FWMAV::Actuator::UpdateTorque() 43 | { 44 | psi_dot_ = *stroke_velocity_; 45 | psi_ddot_ = *stroke_acceleration_; 46 | motor_vel_ = psi_dot_*k_gear_ratio_; 47 | motor_accel_ = psi_ddot_*k_gear_ratio_; 48 | if (psi_dot_>0) 49 | sign_ = 1; 50 | else if (psi_dot_<0) 51 | sign_ = -1; 52 | else 53 | sign_ = 0; 54 | 55 | if (variable_resistance_ == 0) 56 | resistance_ = k_resistance_; 57 | else 58 | resistance_ = variable_resistance_; 59 | 60 | back_EMF_ = k_torque_constant_*motor_vel_; 61 | current_ = (voltage_-back_EMF_)/resistance_; 62 | 63 | inertia_torque_ = k_inertia_*motor_accel_; 64 | damping_torque_ = k_damping_coefficient_*motor_vel_; 65 | friction_torque_ = k_friction_coefficient_*sign_; 66 | magnetic_torque_ = k_torque_constant_*current_; 67 | 68 | motor_torque_ = magnetic_torque_-inertia_torque_-damping_torque_-friction_torque_; 69 | 70 | output_torque_ = motor_torque_*k_gear_ratio_*k_mechanical_efficiency_; 71 | //std::cout << "inertia_torque_=" << inertia_torque_ << " damping_torque_=" << damping_torque_ << " friction_torque_=" << friction_torque_ << " magnetic_torque_=" << magnetic_torque_ << " motor_torque_=" << motor_torque_ << " output_torque_=" << output_torque_ << std::endl; 72 | } 73 | 74 | double FWMAV::Actuator::GetTorque() 75 | { 76 | return output_torque_; 77 | } 78 | 79 | // for debugging 80 | 81 | double FWMAV::Actuator::GetMotorTorque() 82 | { 83 | return motor_torque_; 84 | } 85 | 86 | double FWMAV::Actuator::GetMagTorque() 87 | { 88 | return magnetic_torque_; 89 | } 90 | 91 | double FWMAV::Actuator::GetInerTorque() 92 | { 93 | return inertia_torque_; 94 | } 95 | 96 | double FWMAV::Actuator::GetDampTorque() 97 | { 98 | return damping_torque_; 99 | } 100 | 101 | double FWMAV::Actuator::GetFricTorque() 102 | { 103 | return friction_torque_; 104 | } 105 | 106 | double FWMAV::Actuator::GetBEMF() 107 | { 108 | return back_EMF_; 109 | } 110 | 111 | double FWMAV::Actuator::GetCurrent() 112 | { 113 | return current_; 114 | } 115 | 116 | /* 117 | I_a = (V-K_a psi_dot)/(R_a) 118 | 119 | T_motor = T_load/(eta*N_g) 120 | */ 121 | -------------------------------------------------------------------------------- /test.py: -------------------------------------------------------------------------------- 1 | ########################## FWMAV Simulation ######################### 2 | # Version 0.3 3 | # Fan Fei Feb 2019 4 | # Direct motor driven flapping wing MAV simulation 5 | ####################################################################### 6 | 7 | import gym 8 | import flappy 9 | 10 | from stable_baselines.common.policies import MlpPolicy 11 | from stable_baselines.common.vec_env import DummyVecEnv 12 | from stable_baselines.common.vec_env import SubprocVecEnv 13 | from stable_baselines.common import set_global_seeds 14 | 15 | from flappy.envs.fwmav.controllers.arc_xy_arc_z import ARCController 16 | from flappy.envs.fwmav.controllers.pid_controller import PIDController 17 | 18 | import time 19 | import argparse 20 | import importlib 21 | import numpy as np 22 | 23 | def make_env(env_id, rank, seed=0, random_init = True, randomize_sim = True, phantom_sensor = False): 24 | def _init(): 25 | env = gym.make(env_id) 26 | env.config(random_init, randomize_sim, phantom_sensor) 27 | if rank == 0: 28 | env.enable_visualization() 29 | env.enable_print() 30 | env.seed(seed + rank) 31 | return env 32 | 33 | # set_global_seeds(seed) 34 | return _init 35 | 36 | class LazyModel: 37 | def __init__(self,env,model_type): 38 | self.action_lb = env.action_lb 39 | self.action_ub = env.action_ub 40 | self.observation_bound = env.observation_bound 41 | if model_type == 'PID': 42 | self.policy = PIDController(env.sim.dt_c) 43 | elif model_type == 'ARC': 44 | self.policy = ARCController(env.sim.dt_c) 45 | else: 46 | raise Exception('Error') 47 | 48 | def predict(self, obs): 49 | action = self.policy.get_action(obs[0]*self.observation_bound) 50 | # scale action from [action_lb, action_ub] to [-1,1] 51 | # since baseline does not support asymmetric action space 52 | normalized_action = (action-self.action_lb)/(self.action_ub - self.action_lb)*2 - 1 53 | action = np.array([normalized_action]) 54 | return action, None 55 | 56 | def main(args): 57 | env_id = 'fwmav_hover-v0' 58 | 59 | env = DummyVecEnv([make_env(env_id, 0, random_init = args.rand_init, randomize_sim = args.rand_dynamics, phantom_sensor = args.phantom_sensor)]) 60 | 61 | if args.model_type != 'PID' and args.model_type != 'ARC': 62 | try: 63 | model_cls = getattr( 64 | importlib.import_module('stable_baselines'), args.model_type) 65 | except AttributeError: 66 | print(args.model_type, "Error: wrong model type") 67 | return 68 | try: 69 | model = model_cls.load(args.model_path) 70 | except: 71 | print(args.model_path, "Error: wrong model path") 72 | else: 73 | model = LazyModel(env.envs[0],args.model_type) 74 | 75 | obs = env.reset() 76 | 77 | while True: 78 | if env.envs[0].is_sim_on == False: 79 | env.envs[0].gui.cv.wait() 80 | elif env.envs[0].is_sim_on: 81 | action, _ = model.predict(obs) 82 | obs, rewards, done, info = env.step(action) 83 | # if done: 84 | # obs = env.reset() 85 | 86 | if __name__ == '__main__': 87 | parser = argparse.ArgumentParser() 88 | parser.add_argument('--model_type', required=True) 89 | parser.add_argument('--model_path') 90 | parser.add_argument( 91 | '--policy_type', const='MlpPolicy', default='MlpPolicy', nargs='?') 92 | parser.add_argument('--rand_init', action='store_true', default=False) 93 | parser.add_argument('--rand_dynamics', action='store_true', default=False) 94 | parser.add_argument('--phantom_sensor', action='store_true', default=False) 95 | 96 | args = parser.parse_args() 97 | 98 | main(args) -------------------------------------------------------------------------------- /flappy/envs/fwmav/config/mav_config_list.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "id": 0, 4 | "name": "flapper_sc_nominal", 5 | "urdf_file": "flappy/urdf/fwmav/flapper_sc_nominal.urdf", 6 | "frequency": 34, 7 | "wing_length": 0.07, 8 | "mean_chord": 0.021212121212121, 9 | "r33": 0.205833311654341, 10 | "r22": 0.284203623407408, 11 | "r11": 0.450820740740741, 12 | "r00": 0.999407407407407, 13 | "z_cp2": 0.246352600140835, 14 | "z_cp1": 0.415846717980795, 15 | "z_cp0": 1.028078968449933, 16 | "z_rd": 1.174547978303502, 17 | "left_shoulder_width": 13.95E-3, 18 | "right_shoulder_width": 13.95E-3, 19 | "stroke_plane_offset": 18.9913E-3, 20 | "left_spring_stiffness": 0.013384289, 21 | "right_spring_stiffness": 0.013384289, 22 | "left_stroke_lower": -1.63, 23 | "left_stroke_upper": 1.63, 24 | "left_rotate_lower": -0.7854, 25 | "left_rotate_upper": 0.7854, 26 | "right_stroke_lower": -1.63, 27 | "right_stroke_upper": 1.63, 28 | "right_rotate_lower": -0.7854, 29 | "right_rotate_upper": 0.7854, 30 | "left_stroke_mid": 0.0, 31 | "right_stroke_mid": 0.0, 32 | "left_motor_properties": 33 | { 34 | "resistance": 12.4, 35 | "torque_constant": 1.75e-3, 36 | "gear_ratio": 10, 37 | "mechanical_efficiency": 0.9, 38 | "friction_coefficient": 2e-5, 39 | "damping_coefficient": 9.74e-9, 40 | "inertia": 7.03e-10 41 | }, 42 | "right_motor_properties": 43 | { 44 | "resistance": 12.4, 45 | "torque_constant": 1.75e-3, 46 | "gear_ratio": 10, 47 | "mechanical_efficiency": 0.9, 48 | "friction_coefficient": 2e-5, 49 | "damping_coefficient": 9.74e-9, 50 | "inertia": 7.03e-10 51 | } 52 | }, 53 | { 54 | "id": 1, 55 | "name": "flapper_sc_trim", 56 | "urdf_file": "flappy/urdf/fwmav/flapper_sc_nominal.urdf", 57 | "frequency": 34, 58 | "wing_length": 0.07, 59 | "mean_chord": 0.021212121212121, 60 | "r33": 0.205833311654341, 61 | "r22": 0.284203623407408, 62 | "r11": 0.450820740740741, 63 | "r00": 0.999407407407407, 64 | "z_cp2": 0.246352600140835, 65 | "z_cp1": 0.415846717980795, 66 | "z_cp0": 1.028078968449933, 67 | "z_rd": 1.174547978303502, 68 | "left_shoulder_width": 13.95E-3, 69 | "right_shoulder_width": 13.95E-3, 70 | "stroke_plane_offset": 18.9913E-3, 71 | "left_spring_stiffness": 0.0133599752, 72 | "right_spring_stiffness": 0.0133000000, 73 | "left_stroke_lower": -1.63, 74 | "left_stroke_upper": 1.63, 75 | "left_rotate_lower": -0.844856704, 76 | "left_rotate_upper": 0.765164977, 77 | "right_stroke_lower": -1.63, 78 | "right_stroke_upper": 1.63, 79 | "right_rotate_lower": -0.854799666, 80 | "right_rotate_upper": 0.814000120, 81 | "left_stroke_mid": 0.0403463879, 82 | "right_stroke_mid": 0.0319281979, 83 | "left_motor_properties": 84 | { 85 | "resistance": 14.780065776, 86 | "torque_constant": 1.75e-3, 87 | "gear_ratio": 10, 88 | "mechanical_efficiency": 0.9, 89 | "friction_coefficient": 2e-5, 90 | "damping_coefficient": 9.74e-9, 91 | "inertia": 7.03e-10 92 | }, 93 | "right_motor_properties": 94 | { 95 | "resistance": 13.763265084, 96 | "torque_constant": 1.75e-3, 97 | "gear_ratio": 10, 98 | "mechanical_efficiency": 0.9, 99 | "friction_coefficient": 2e-5, 100 | "damping_coefficient": 9.74e-9, 101 | "inertia": 7.03e-10 102 | } 103 | } 104 | ] -------------------------------------------------------------------------------- /flappy/envs/fwmav/ddpg_fwmav.py: -------------------------------------------------------------------------------- 1 | ########################## FWMAV Simulation ######################### 2 | # Version 0.2 3 | # Fan Fei Mar 2018 4 | # FWMAV simulation with simplified aerodynamics 5 | ####################################################################### 6 | 7 | 8 | from rllab.algos.ddpg import DDPG 9 | from rllab.envs.normalized_env import normalize 10 | from rllab.exploration_strategies.ou_strategy import OUStrategy 11 | from rllab.policies.deterministic_mlp_policy import DeterministicMLPPolicy 12 | from rllab.q_functions.continuous_mlp_q_function import ContinuousMLPQFunction 13 | import lasagne.nonlinearities as NL 14 | 15 | from fwmav_sim_env_maneuver import FWMAVSimEnv 16 | import os 17 | import rllab.misc.logger as logger 18 | import pickle 19 | import sys 20 | import lasagne.init as LI 21 | 22 | env = normalize(FWMAVSimEnv()) 23 | policy = DeterministicMLPPolicy( 24 | env_spec=env.spec, 25 | hidden_nonlinearity=NL.tanh,#NL.rectify,LeakyRectify 26 | output_nonlinearity=NL.tanh, 27 | hidden_sizes=(32, 32), 28 | 29 | ) 30 | 31 | es = OUStrategy(env_spec=env.spec, theta = 0.15, sigma = 0.3) #theta = decay rate of noise (small decay slower, fluctuate more, theta = 0.01 is about 220 steps, theta = 0.1 is about 20 steps, 0.15 is 15 step, 0.022 is 100 step), sigma = variation or the size of the noise 32 | 33 | qf = ContinuousMLPQFunction( 34 | env_spec=env.spec, 35 | hidden_nonlinearity=NL.tanh, 36 | output_nonlinearity=None, 37 | hidden_sizes=(128, 128), 38 | output_W_init=LI.Uniform(-3e-6, 3e-6), 39 | output_b_init=LI.Uniform(-3e-6, 3e-6), 40 | ) 41 | 42 | algo = DDPG( 43 | env=env, 44 | policy=policy, 45 | es=es, 46 | qf=qf, 47 | batch_size=256, # Number of samples for each minibatch. 48 | max_path_length=1500, # 5 seconds 49 | epoch_length=15000, # How many timesteps for each epoch. 50 | min_pool_size=15000, # Minimum size of the pool to start training. 51 | replay_pool_size=15000000, 52 | n_epochs=1000, # Number of epochs. Policy will be evaluated after each epoch. 53 | eval_samples=15000, # Number of samples (timesteps) for evaluating the policy. 54 | discount=1.0, 55 | scale_reward=0.1, # The scaling factor applied to the rewards when training 56 | qf_learning_rate=1e-3, # Learning rate for training Q function 57 | policy_learning_rate=1e-4, # Learning rate for training the policy 58 | #qf_weight_decay=0.01, 59 | soft_target_tau=0.005, # Interpolation parameter for doing the soft target update. 60 | # Uncomment both lines (this and the plot parameter below) to enable plotting 61 | # plot=True, 62 | ) 63 | 64 | log_dir = os.path.join(os.getcwd(),'data') 65 | logger.set_snapshot_dir(log_dir) 66 | logger.add_text_output(os.path.join(log_dir,'debug.log')) 67 | logger.add_tabular_output(os.path.join(log_dir,'progress.csv')) 68 | logger.set_snapshot_mode('last') 69 | 70 | algo.train() 71 | 72 | # save parameters 73 | with open(os.path.join(log_dir,'final_policy.pkl'), 'wb') as output: 74 | trained_policy = algo.policy 75 | pickle.dump(trained_policy, output, pickle.HIGHEST_PROTOCOL) 76 | print('Final policy saved') 77 | 78 | def save_large_pickled_object(obj, filepath): 79 | """ 80 | This is a defensive way to write pickle.write, allowing for very large files on all platforms 81 | """ 82 | max_bytes = 2**31 - 1 83 | bytes_out = pickle.dumps(obj) 84 | n_bytes = sys.getsizeof(bytes_out) 85 | with open(filepath, 'wb') as f_out: 86 | for idx in range(0, n_bytes, max_bytes): 87 | f_out.write(bytes_out[idx:idx+max_bytes]) 88 | 89 | pool = algo.pool 90 | save_large_pickled_object(pool,os.path.join(log_dir,'final_pool.pkl')) 91 | print('Final pool saved') 92 | -------------------------------------------------------------------------------- /train_DDPG.py: -------------------------------------------------------------------------------- 1 | ########################## FWMAV Simulation ######################### 2 | # Version 0.3 3 | # Fan Fei Feb 2019 4 | # Direct motor driven flapping wing MAV simulation 5 | ####################################################################### 6 | 7 | import gym 8 | import flappy 9 | import numpy as np 10 | import tensorflow as tf 11 | 12 | from stable_baselines.ddpg.policies import FeedForwardPolicy 13 | from stable_baselines.common.vec_env import DummyVecEnv 14 | from stable_baselines.common.vec_env import SubprocVecEnv 15 | from stable_baselines.common import set_global_seeds 16 | from stable_baselines.ddpg.noise import NormalActionNoise, OrnsteinUhlenbeckActionNoise, AdaptiveParamNoiseSpec 17 | from stable_baselines import DDPG 18 | 19 | import time 20 | import argparse 21 | import importlib 22 | 23 | 24 | class MyDDPGPolicy(FeedForwardPolicy): 25 | def __init__(self, sess, ob_space, ac_space, n_env, n_steps, n_batch, reuse=False, **_kwargs): 26 | super(MyDDPGPolicy, self).__init__(sess, ob_space, ac_space, n_env, n_steps, n_batch, reuse, 27 | act_fun=tf.nn.tanh, layers=[32, 32], feature_extraction="mlp", **_kwargs) 28 | 29 | def make_critic(self, obs=None, action=None, reuse=False, scope="qf"): 30 | critic_layers = [128, 128] 31 | 32 | if obs is None: 33 | obs = self.processed_obs 34 | if action is None: 35 | action = self.action_ph 36 | 37 | with tf.variable_scope(scope, reuse=reuse): 38 | qf_h = tf.layers.flatten(obs) 39 | 40 | for i, layer_size in enumerate(critic_layers): 41 | qf_h = tf.layers.dense(qf_h, layer_size, name='fc' + str(i)) 42 | if self.layer_norm: 43 | qf_h = tf.contrib.layers.layer_norm(qf_h, center=True, scale=True) 44 | qf_h = self.activ(qf_h) 45 | if i == 0: 46 | qf_h = tf.concat([qf_h, action], axis=-1) 47 | 48 | qvalue_fn = tf.layers.dense(qf_h, 1, name=scope, 49 | kernel_initializer=tf.random_uniform_initializer(minval=-3e-3, 50 | maxval=3e-3)) 51 | 52 | self.qvalue_fn = qvalue_fn 53 | self._qvalue = qvalue_fn[:, 0] 54 | return self.qvalue_fn 55 | 56 | def make_env(env_id, rank, seed=0, random_init = True, randomize_sim = True, phantom_sensor = False): 57 | def _init(): 58 | env = gym.make(env_id) 59 | env.config(random_init, randomize_sim, phantom_sensor) 60 | if rank == 0: 61 | env.enable_visualization() 62 | env.enable_print() 63 | env.seed(seed + rank) 64 | return env 65 | 66 | # set_global_seeds(seed) 67 | return _init 68 | 69 | def main(args): 70 | 71 | start = time.time() 72 | 73 | env_id = 'fwmav_hover-v0' 74 | env = DummyVecEnv([make_env(env_id, 0)]) 75 | # env = SubprocVecEnv([make_env(env_id, i) for i in range(args.n_cpu)]) 76 | 77 | n_actions = env.action_space.shape[-1] 78 | param_noise = None 79 | action_noise = OrnsteinUhlenbeckActionNoise(mean=np.zeros(n_actions), sigma=float(0.5) * np.ones(n_actions)) 80 | 81 | model = DDPG( 82 | policy = MyDDPGPolicy, 83 | env = env, 84 | gamma = 1.0, 85 | nb_train_steps=5000, 86 | nb_rollout_steps=10000, 87 | nb_eval_steps=10000, 88 | param_noise=param_noise, 89 | action_noise=action_noise, 90 | tau=0.003, 91 | batch_size=256, 92 | observation_range=(-np.inf, np.inf), 93 | actor_lr=0.0001, 94 | critic_lr=0.001, 95 | reward_scale=0.05, 96 | memory_limit=10000000, 97 | verbose=1, 98 | ) 99 | 100 | model.learn(total_timesteps=args.time_step) 101 | model.save(args.model_path) 102 | 103 | end = time.time() 104 | print("Time used: ", end - start) 105 | 106 | 107 | if __name__ == '__main__': 108 | parser = argparse.ArgumentParser() 109 | parser.add_argument('--model_path', required=True, nargs='?') 110 | parser.add_argument('--time_step', required=True, type=int, nargs='?') 111 | args = parser.parse_args() 112 | 113 | main(args) 114 | -------------------------------------------------------------------------------- /train_maneuver_DDPG.py: -------------------------------------------------------------------------------- 1 | ########################## FWMAV Simulation ######################### 2 | # Version 0.3 3 | # Fan Fei Feb 2019 4 | # Direct motor driven flapping wing MAV simulation 5 | ####################################################################### 6 | 7 | import gym 8 | import flappy 9 | import numpy as np 10 | import tensorflow as tf 11 | 12 | from stable_baselines.ddpg.policies import FeedForwardPolicy 13 | from stable_baselines.common.vec_env import DummyVecEnv 14 | from stable_baselines.common.vec_env import SubprocVecEnv 15 | from stable_baselines.common import set_global_seeds 16 | from stable_baselines.ddpg.noise import NormalActionNoise, OrnsteinUhlenbeckActionNoise, AdaptiveParamNoiseSpec 17 | from stable_baselines import DDPG 18 | 19 | import time 20 | import argparse 21 | import importlib 22 | 23 | 24 | class MyDDPGPolicy(FeedForwardPolicy): 25 | def __init__(self, sess, ob_space, ac_space, n_env, n_steps, n_batch, reuse=False, **_kwargs): 26 | super(MyDDPGPolicy, self).__init__(sess, ob_space, ac_space, n_env, n_steps, n_batch, reuse, 27 | act_fun=tf.nn.tanh, layers=[32, 32], feature_extraction="mlp", **_kwargs) 28 | 29 | def make_critic(self, obs=None, action=None, reuse=False, scope="qf"): 30 | critic_layers = [128, 128] 31 | 32 | if obs is None: 33 | obs = self.processed_obs 34 | if action is None: 35 | action = self.action_ph 36 | 37 | with tf.variable_scope(scope, reuse=reuse): 38 | qf_h = tf.layers.flatten(obs) 39 | 40 | for i, layer_size in enumerate(critic_layers): 41 | qf_h = tf.layers.dense(qf_h, layer_size, name='fc' + str(i)) 42 | if self.layer_norm: 43 | qf_h = tf.contrib.layers.layer_norm(qf_h, center=True, scale=True) 44 | qf_h = self.activ(qf_h) 45 | if i == 0: 46 | qf_h = tf.concat([qf_h, action], axis=-1) 47 | 48 | qvalue_fn = tf.layers.dense(qf_h, 1, name=scope, 49 | kernel_initializer=tf.random_uniform_initializer(minval=-3e-3, 50 | maxval=3e-3)) 51 | 52 | self.qvalue_fn = qvalue_fn 53 | self._qvalue = qvalue_fn[:, 0] 54 | return self.qvalue_fn 55 | 56 | def make_env(env_id, rank, seed=0, random_init = True, randomize_sim = True, phantom_sensor = False): 57 | def _init(): 58 | env = gym.make(env_id) 59 | env.config(random_init, randomize_sim, phantom_sensor) 60 | if rank == 0: 61 | env.enable_visualization() 62 | env.enable_print() 63 | env.seed(seed + rank) 64 | return env 65 | 66 | # set_global_seeds(seed) 67 | return _init 68 | 69 | def main(args): 70 | 71 | start = time.time() 72 | 73 | env_id = 'fwmav_maneuver-v0' 74 | env = DummyVecEnv([make_env(env_id, 0)]) 75 | # env = SubprocVecEnv([make_env(env_id, i) for i in range(args.n_cpu)]) 76 | 77 | n_actions = env.action_space.shape[-1] 78 | param_noise = None 79 | action_noise = OrnsteinUhlenbeckActionNoise(mean=np.zeros(n_actions), sigma=float(0.5) * np.ones(n_actions)) 80 | 81 | model = DDPG( 82 | policy = MyDDPGPolicy, 83 | env = env, 84 | gamma = 1.0, 85 | nb_train_steps=5000, 86 | nb_rollout_steps=10000, 87 | nb_eval_steps=10000, 88 | param_noise=param_noise, 89 | action_noise=action_noise, 90 | tau=0.003, 91 | batch_size=256, 92 | observation_range=(-np.inf, np.inf), 93 | actor_lr=0.0001, 94 | critic_lr=0.001, 95 | reward_scale=0.05, 96 | memory_limit=10000000, 97 | verbose=1, 98 | ) 99 | 100 | model.learn(total_timesteps=args.time_step) 101 | model.save(args.model_path) 102 | 103 | end = time.time() 104 | print("Time used: ", end - start) 105 | 106 | 107 | if __name__ == '__main__': 108 | parser = argparse.ArgumentParser() 109 | parser.add_argument('--model_path', required=True, nargs='?') 110 | parser.add_argument('--time_step', required=True, type=int, nargs='?') 111 | args = parser.parse_args() 112 | 113 | main(args) 114 | -------------------------------------------------------------------------------- /test_simple.py: -------------------------------------------------------------------------------- 1 | ########################## FWMAV Simulation ######################### 2 | # Version 0.3 3 | # Fan Fei Feb 2019 4 | # Direct motor driven flapping wing MAV simulation 5 | ####################################################################### 6 | 7 | import gym 8 | import flappy 9 | 10 | from stable_baselines.common.policies import MlpPolicy 11 | from stable_baselines.common.vec_env import DummyVecEnv 12 | from stable_baselines.common.vec_env import SubprocVecEnv 13 | from stable_baselines.common import set_global_seeds 14 | 15 | from flappy.envs.fwmav.controllers.arc_xy_arc_z import ARCController 16 | from flappy.envs.fwmav.controllers.pid_controller import PIDController 17 | 18 | import time 19 | import argparse 20 | import importlib 21 | import numpy as np 22 | 23 | def make_env(env_id, rank, seed=0, random_init = True, randomize_sim = True, phantom_sensor = False): 24 | def _init(): 25 | env = gym.make(env_id) 26 | env.config(random_init, randomize_sim, phantom_sensor) 27 | if rank == 0: 28 | env.enable_visualization() 29 | env.enable_print() 30 | env.seed(seed + rank) 31 | return env 32 | 33 | # set_global_seeds(seed) 34 | return _init 35 | 36 | class LazyModel: 37 | def __init__(self,env,model_type): 38 | self.action_lb = env.action_lb 39 | self.action_ub = env.action_ub 40 | self.observation_bound = env.observation_bound 41 | self.frequency = 34 42 | self.env = env 43 | if model_type == 'PID': 44 | self.policy = PIDController(env.sim.dt_c) 45 | elif model_type == 'ARC': 46 | self.policy = ARCController(env.sim.dt_c) 47 | else: 48 | raise Exception('Error') 49 | 50 | def predict(self, obs): 51 | time = self.env.sim.world.time() 52 | action = self.policy.get_action(obs[0]*self.observation_bound) 53 | 54 | max_voltage = action[0] 55 | voltage_diff = action[1] 56 | voltage_bias = action[2] 57 | split_cycle = action[3] 58 | 59 | input_voltage = np.zeros([2],dtype=np.float64) 60 | input_voltage[0] = self.generate_control_signal(self.frequency, max_voltage, voltage_diff, voltage_bias, -split_cycle, time, 0) 61 | input_voltage[1] = self.generate_control_signal(self.frequency, max_voltage, -voltage_diff, voltage_bias, split_cycle, time, 0) 62 | 63 | # scale action from [action_lb, action_ub] to [-1,1] 64 | # since baseline does not support asymmetric action space 65 | normalized_action = (input_voltage-self.action_lb)/(self.action_ub - self.action_lb)*2 - 1 66 | action = np.array([normalized_action]) 67 | return action, None 68 | 69 | def generate_control_signal(self, f, Umax, delta, bias, sc, t, phase_0): 70 | V = Umax + delta 71 | V0 = bias 72 | sigma = 0.5+sc 73 | 74 | T = 1/f 75 | t_phase = phase_0/360*T 76 | t = t+t_phase 77 | period = np.floor(t/T) 78 | t = t-period*T 79 | 80 | if 0<=t and t 9 | 10 | namespace FWMAV{ 11 | class Wing { 12 | public: 13 | Wing(int wing_index, 14 | double wing_length, 15 | double mean_chord, 16 | double r33, 17 | double r22, 18 | double r11, 19 | double r00, 20 | double z_cp2, 21 | double z_cp1, 22 | double z_cp0, 23 | double z_rd, 24 | double shoulder_width, 25 | double stroke_plane_offset); 26 | virtual ~Wing(); 27 | 28 | void doNothing(); 29 | 30 | void UpdateAeroForce(); 31 | 32 | void UpdateStates( double body_velocity_roll, 33 | double body_velocity_pitch, 34 | double body_velocity_yaw, 35 | double body_velocity_x, 36 | double body_velocity_y, 37 | double body_velocity_z, 38 | double stroke_plane_angle, 39 | double stroke_plane_velocity, 40 | double stroke_angle, 41 | double stroke_velocity, 42 | double deviation_angle, 43 | double deviation_velocity, 44 | double rotate_angle, 45 | double rotate_velocity); 46 | void UpdateVelocityCoeff(); 47 | void UpdateAoA(); 48 | void UpdateCN(); 49 | void UpdateVelocitySquaredCoeff(); 50 | void UpdateCenterOfPressure(); 51 | //void UpdateMomentCoeff(); 52 | 53 | // grabber functions 54 | double GetSpanCoP(); 55 | double GetChordCoP(); 56 | double GetNormalForce(); 57 | double GetMoment(); 58 | double GetM_aero(); 59 | double GetM_rd(); 60 | 61 | // math functions 62 | double C_N(double alpha); //CN 63 | //double dC_N(double alpha); //partial CN / partial alpha 64 | double d_cp(double alpha); //Dickenson center of pressure Fourier approximation 65 | //double dd_cp(double alpha); 66 | template int sgn(T val); 67 | 68 | //tester function 69 | double GetStroke(); 70 | double GetAoA(); 71 | 72 | protected: 73 | 74 | // wing type and velocity pointers 75 | int sign_; 76 | 77 | // double* body_velocity_roll_; 78 | // double* body_velocity_pitch_; 79 | // double* body_velocity_yaw_; 80 | // double* body_velocity_x_; 81 | // double* body_velocity_y_; 82 | // double* body_velocity_z_; 83 | // double* stroke_angle_; 84 | // double* stroke_velocity_; 85 | // double* rotate_angle_; 86 | // double* rotate_velocity_; 87 | 88 | // flapper geometry 89 | double R_w_; // wing length 90 | double d_0_; // half shoulder 91 | double d_s_; // stroke plane offset 92 | 93 | // wing geometry constants 94 | double air_density_; 95 | double wing_length_; 96 | double mean_chord_; 97 | double r33_; 98 | double r22_; 99 | double r11_; 100 | double r00_; 101 | double z_cp2_; 102 | double z_cp1_; 103 | double z_cp0_; 104 | double z_rd_; 105 | 106 | // body velocities 107 | double u_; // x 108 | double v_; // y 109 | double w_; // z 110 | double p_; // roll 111 | double q_; // pitch 112 | double r_; // yaw 113 | 114 | // wing angle and velocities 115 | double Phi_; // stroke plane angle 116 | double Phi_dot_; 117 | double psi_; // stroke angle 118 | double psi_dot_; 119 | double phi_; // deviation angle 120 | double phi_dot_; 121 | double theta_; // wing rotation angle 122 | double theta_dot_; 123 | 124 | // wing angle trig 125 | double S_Phi_; 126 | double C_Phi_; 127 | double S_psi_; 128 | double C_psi_; 129 | double S_phi_; 130 | double C_phi_; 131 | 132 | // velocity coefficients 133 | double U_o1_; 134 | double U_o0_; 135 | double U_i1_; 136 | double U_i0_; 137 | 138 | // AoA 139 | double delta_alpha_; 140 | double alpha_0_; 141 | double alpha_; 142 | 143 | // C_N 144 | double C_N_; 145 | 146 | // velocity squared coefficients 147 | double a_U2_; 148 | double a_U1_; 149 | double a_U0_; 150 | 151 | // F_N coefficients 152 | double K_N2_; 153 | double K_N1_; 154 | double K_N0_; 155 | double K_N_1_; 156 | 157 | // center of pressure 158 | double d_cp_; 159 | double r_cp_; 160 | 161 | // moment coefficients 162 | double K_aero2_; 163 | double K_aero1_; 164 | double K_aero0_; 165 | double K_aero_1_; 166 | double K_aero_2_; 167 | 168 | // total force and moments 169 | double span_wise_center_of_pressure_; 170 | double cord_wise_center_of_pressure_; 171 | double normal_force_; 172 | double aero_moment_; 173 | double rotational_damping_moment_; 174 | 175 | }; 176 | } -------------------------------------------------------------------------------- /flappy/envs/fwmav/sensor_for_no_pos.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class Sensor_IMU_Vicon(): 4 | def __init__(self, dt_imu, dt_vicon, states): 5 | self.dt_imu_ = dt_imu 6 | self.dt_vicon_ = dt_vicon 7 | 8 | vicon_delay_time_ = 0.030 9 | self.vicon_roll_noise_ = 0.005 # 0.00873 = 0.5 degree 10 | self.vicon_pitch_noise_ = 0.005 11 | self.vicon_yaw_noise_ = 0.005 12 | self.vicon_x_noise_ = 1e-4 # 0.5mm; 13 | self.vicon_y_noise_ = 1e-4 14 | self.vicon_z_noise_ = 1e-4 15 | self.IMU_ax_noise_ = 100.0 # 1m/s^2 16 | self.IMU_ay_noise_ = 100.0 17 | self.IMU_az_noise_ = 100.0 18 | 19 | self.IMU_gx_noise_ = 3/180*np.pi # 0.00873 = 0.5 deg/s 20 | self.IMU_gy_noise_ = 3/180*np.pi 21 | self.IMU_gz_noise_ = 3/180*np.pi 22 | self.IMU_mx_noise_ = 50 23 | self.IMU_my_noise_ = 50 24 | self.IMU_mz_noise_ = 50 25 | 26 | self.delay_vicon_step_ = np.ceil(vicon_delay_time_/dt_vicon); 27 | 28 | self.dt_vicon_actual_ = dt_vicon 29 | self.dt_IMU_actual_ = dt_imu 30 | self.reset(states) 31 | 32 | 33 | def reset(self,states): 34 | self.IMU_gx_noise_mean_ = np.random.normal(0,0.02) #0.03 35 | self.IMU_gy_noise_mean_ = np.random.normal(0,0.03) #-0.035 # -0.035 = -1.988deg/s this non zero mean is the drift effect 36 | self.IMU_gz_noise_mean_ = np.random.normal(0,0.015) #0.02 37 | self.last_vicon_update_time_ = 0 38 | self.next_vicon_update_time_ = 0 39 | self.last_IMU_update_time_ = 0 40 | self.next_IMU_update_time_ = 0 41 | self.vicon_roll_cache_ = np.zeros([self.delay_vicon_step_.astype(int)],dtype=np.float64) + states['body_positions'][0,0] 42 | self.vicon_pitch_cache_ = np.zeros([self.delay_vicon_step_.astype(int)],dtype=np.float64) + states['body_positions'][1,0] 43 | self.vicon_yaw_cache_ = np.zeros([self.delay_vicon_step_.astype(int)],dtype=np.float64) + states['body_positions'][2,0] 44 | self.vicon_x_cache_ = np.zeros([self.delay_vicon_step_.astype(int)],dtype=np.float64) + states['body_positions'][3,0] 45 | self.vicon_y_cache_ = np.zeros([self.delay_vicon_step_.astype(int)],dtype=np.float64) + states['body_positions'][4,0] 46 | self.vicon_z_cache_ = np.zeros([self.delay_vicon_step_.astype(int)],dtype=np.float64) + states['body_positions'][5,0] 47 | print(states['body_positions'][3,0], end="\n\r") 48 | self.vicon_cache_pointer = 0 49 | self.vicon_update_flag_ = 0 50 | 51 | def update_raw_data(self, time, states): 52 | self.update_Vicon(time, states) 53 | self.update_IMU(time, states) 54 | 55 | def update_Vicon(self, time, states): 56 | if time >= self.next_vicon_update_time_: 57 | self.dt_vicon_actual_ = time - self.last_vicon_update_time_ 58 | 59 | roll_reading = states['body_positions'][0,0] + np.random.normal(0,self.vicon_roll_noise_) 60 | pitch_reading = states['body_positions'][1,0] + np.random.normal(0,self.vicon_pitch_noise_) 61 | yaw_reading = states['body_positions'][2,0] + np.random.normal(0,self.vicon_yaw_noise_) 62 | x_reading = states['body_positions'][3,0] + np.random.normal(0,self.vicon_x_noise_) 63 | y_reading = states['body_positions'][4,0] + np.random.normal(0,self.vicon_y_noise_) 64 | z_reading = states['body_positions'][5,0] + np.random.normal(0,self.vicon_z_noise_) 65 | 66 | # cache reading 67 | self.vicon_roll_cache_[self.vicon_cache_pointer] = roll_reading 68 | self.vicon_pitch_cache_[self.vicon_cache_pointer] = pitch_reading 69 | self.vicon_yaw_cache_[self.vicon_cache_pointer] = yaw_reading 70 | self.vicon_x_cache_[self.vicon_cache_pointer] = x_reading 71 | self.vicon_y_cache_[self.vicon_cache_pointer] = y_reading 72 | self.vicon_z_cache_[self.vicon_cache_pointer] = z_reading 73 | 74 | self.vicon_read_pointer = self.vicon_cache_pointer + 1 75 | if self.vicon_read_pointer >= self.delay_vicon_step_: 76 | self.vicon_read_pointer = 0 77 | 78 | # output delayed reading 79 | self.vicon_roll_ = self.vicon_roll_cache_[self.vicon_read_pointer] 80 | self.vicon_pitch_ = self.vicon_pitch_cache_[self.vicon_read_pointer] 81 | self.vicon_yaw_ = self.vicon_yaw_cache_[self.vicon_read_pointer] 82 | self.vicon_x_ = self.vicon_x_cache_[self.vicon_read_pointer] 83 | self.vicon_y_ = self.vicon_y_cache_[self.vicon_read_pointer] 84 | self.vicon_z_ = self.vicon_z_cache_[self.vicon_read_pointer] 85 | 86 | self.vicon_cache_pointer = self.vicon_cache_pointer + 1 87 | if self.vicon_cache_pointer >= self.delay_vicon_step_: 88 | self.vicon_cache_pointer = 0 89 | 90 | self.last_vicon_update_time_ = time 91 | self.next_vicon_update_time_ = self.next_vicon_update_time_ + self.dt_vicon_ 92 | 93 | if(time= self.next_IMU_update_time_: 101 | self.dt_IMU_actual_ = time - self.last_IMU_update_time_ 102 | 103 | self.IMU_ax_ = states['body_accelerations'][0,0]*2 + np.random.normal(0,self.IMU_ax_noise_) 104 | self.IMU_ay_ = states['body_accelerations'][1,0]*1.5 + np.random.normal(0,self.IMU_ay_noise_) 105 | self.IMU_az_ = states['body_accelerations'][2,0]*1.5 + np.random.normal(0,self.IMU_az_noise_) 106 | self.IMU_gx_ = states['body_velocities'][0,0] + np.random.normal(self.IMU_gx_noise_mean_,self.IMU_gx_noise_) 107 | self.IMU_gy_ = states['body_velocities'][1,0] + np.random.normal(self.IMU_gy_noise_mean_,self.IMU_gy_noise_) 108 | self.IMU_gz_ = states['body_velocities'][2,0] + np.random.normal(self.IMU_gz_noise_mean_,self.IMU_gz_noise_) 109 | self.IMU_mx_ = np.random.normal(0,self.IMU_mx_noise_) 110 | self.IMU_my_ = np.random.normal(0,self.IMU_my_noise_) 111 | self.IMU_mz_ = np.random.normal(0,self.IMU_mz_noise_) 112 | 113 | self.last_IMU_update_time_ = time 114 | self.next_IMU_update_time_ = self.next_IMU_update_time_ + self.dt_imu_ 115 | #print('IMU updated!', end="\n\r") 116 | 117 | 118 | 119 | -------------------------------------------------------------------------------- /flappy/urdf/fwmav/flapper_sc_nominal.urdf: -------------------------------------------------------------------------------- 1 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | -------------------------------------------------------------------------------- /flappy/envs/Wing.py: -------------------------------------------------------------------------------- 1 | # This file was automatically generated by SWIG (http://www.swig.org). 2 | # Version 3.0.8 3 | # 4 | # Do not make changes to this file unless you know what you are doing--modify 5 | # the SWIG interface file instead. 6 | 7 | 8 | 9 | 10 | 11 | from sys import version_info 12 | if version_info >= (2, 6, 0): 13 | def swig_import_helper(): 14 | from os.path import dirname 15 | import imp 16 | fp = None 17 | try: 18 | fp, pathname, description = imp.find_module('_Wing', [dirname(__file__)]) 19 | except ImportError: 20 | import _Wing 21 | return _Wing 22 | if fp is not None: 23 | try: 24 | _mod = imp.load_module('_Wing', fp, pathname, description) 25 | finally: 26 | fp.close() 27 | return _mod 28 | _Wing = swig_import_helper() 29 | del swig_import_helper 30 | else: 31 | import _Wing 32 | del version_info 33 | try: 34 | _swig_property = property 35 | except NameError: 36 | pass # Python < 2.2 doesn't have 'property'. 37 | 38 | 39 | def _swig_setattr_nondynamic(self, class_type, name, value, static=1): 40 | if (name == "thisown"): 41 | return self.this.own(value) 42 | if (name == "this"): 43 | if type(value).__name__ == 'SwigPyObject': 44 | self.__dict__[name] = value 45 | return 46 | method = class_type.__swig_setmethods__.get(name, None) 47 | if method: 48 | return method(self, value) 49 | if (not static): 50 | if _newclass: 51 | object.__setattr__(self, name, value) 52 | else: 53 | self.__dict__[name] = value 54 | else: 55 | raise AttributeError("You cannot add attributes to %s" % self) 56 | 57 | 58 | def _swig_setattr(self, class_type, name, value): 59 | return _swig_setattr_nondynamic(self, class_type, name, value, 0) 60 | 61 | 62 | def _swig_getattr_nondynamic(self, class_type, name, static=1): 63 | if (name == "thisown"): 64 | return self.this.own() 65 | method = class_type.__swig_getmethods__.get(name, None) 66 | if method: 67 | return method(self) 68 | if (not static): 69 | return object.__getattr__(self, name) 70 | else: 71 | raise AttributeError(name) 72 | 73 | def _swig_getattr(self, class_type, name): 74 | return _swig_getattr_nondynamic(self, class_type, name, 0) 75 | 76 | 77 | def _swig_repr(self): 78 | try: 79 | strthis = "proxy of " + self.this.__repr__() 80 | except Exception: 81 | strthis = "" 82 | return "<%s.%s; %s >" % (self.__class__.__module__, self.__class__.__name__, strthis,) 83 | 84 | try: 85 | _object = object 86 | _newclass = 1 87 | except AttributeError: 88 | class _object: 89 | pass 90 | _newclass = 0 91 | 92 | 93 | class Wing(_object): 94 | __swig_setmethods__ = {} 95 | __setattr__ = lambda self, name, value: _swig_setattr(self, Wing, name, value) 96 | __swig_getmethods__ = {} 97 | __getattr__ = lambda self, name: _swig_getattr(self, Wing, name) 98 | __repr__ = _swig_repr 99 | 100 | def __init__(self, wing_index, wing_length, mean_chord, r33, r22, r11, r00, z_cp2, z_cp1, z_cp0, z_rd, shoulder_width, stroke_plane_offset): 101 | this = _Wing.new_Wing(wing_index, wing_length, mean_chord, r33, r22, r11, r00, z_cp2, z_cp1, z_cp0, z_rd, shoulder_width, stroke_plane_offset) 102 | try: 103 | self.this.append(this) 104 | except Exception: 105 | self.this = this 106 | __swig_destroy__ = _Wing.delete_Wing 107 | __del__ = lambda self: None 108 | 109 | def doNothing(self): 110 | return _Wing.Wing_doNothing(self) 111 | 112 | def UpdateAeroForce(self): 113 | return _Wing.Wing_UpdateAeroForce(self) 114 | 115 | def UpdateStates(self, body_velocity_roll, body_velocity_pitch, body_velocity_yaw, body_velocity_x, body_velocity_y, body_velocity_z, stroke_plane_angle, stroke_plane_velocity, stroke_angle, stroke_velocity, deviation_angle, deviation_velocity, rotate_angle, rotate_velocity): 116 | return _Wing.Wing_UpdateStates(self, body_velocity_roll, body_velocity_pitch, body_velocity_yaw, body_velocity_x, body_velocity_y, body_velocity_z, stroke_plane_angle, stroke_plane_velocity, stroke_angle, stroke_velocity, deviation_angle, deviation_velocity, rotate_angle, rotate_velocity) 117 | 118 | def UpdateVelocityCoeff(self): 119 | return _Wing.Wing_UpdateVelocityCoeff(self) 120 | 121 | def UpdateAoA(self): 122 | return _Wing.Wing_UpdateAoA(self) 123 | 124 | def UpdateCN(self): 125 | return _Wing.Wing_UpdateCN(self) 126 | 127 | def UpdateVelocitySquaredCoeff(self): 128 | return _Wing.Wing_UpdateVelocitySquaredCoeff(self) 129 | 130 | def UpdateCenterOfPressure(self): 131 | return _Wing.Wing_UpdateCenterOfPressure(self) 132 | 133 | def GetSpanCoP(self): 134 | return _Wing.Wing_GetSpanCoP(self) 135 | 136 | def GetChordCoP(self): 137 | return _Wing.Wing_GetChordCoP(self) 138 | 139 | def GetNormalForce(self): 140 | return _Wing.Wing_GetNormalForce(self) 141 | 142 | def GetMoment(self): 143 | return _Wing.Wing_GetMoment(self) 144 | 145 | def GetM_aero(self): 146 | return _Wing.Wing_GetM_aero(self) 147 | 148 | def GetM_rd(self): 149 | return _Wing.Wing_GetM_rd(self) 150 | 151 | def C_N(self, alpha): 152 | return _Wing.Wing_C_N(self, alpha) 153 | 154 | def d_cp(self, alpha): 155 | return _Wing.Wing_d_cp(self, alpha) 156 | 157 | def GetStroke(self): 158 | return _Wing.Wing_GetStroke(self) 159 | 160 | def GetAoA(self): 161 | return _Wing.Wing_GetAoA(self) 162 | Wing_swigregister = _Wing.Wing_swigregister 163 | Wing_swigregister(Wing) 164 | 165 | # This file is compatible with both classic and new-style classes. 166 | 167 | 168 | -------------------------------------------------------------------------------- /flappy/envs/fwmav/sensor.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class Sensor_IMU_Vicon(): 4 | def __init__(self, dt_imu, dt_vicon, states): 5 | self.dt_imu_ = dt_imu 6 | self.dt_vicon_ = dt_vicon 7 | 8 | vicon_delay_time_ = 0.030 9 | self.vicon_roll_noise_ = 0.005 # 0.00873 = 0.5 degree 10 | self.vicon_pitch_noise_ = 0.005 11 | self.vicon_yaw_noise_ = 0.005 12 | self.vicon_x_noise_ = 1e-4 # 0.5mm; 13 | self.vicon_y_noise_ = 1e-4 14 | self.vicon_z_noise_ = 1e-4 15 | self.IMU_ax_noise_ = 0.5 # 1m/s^2 16 | self.IMU_ay_noise_ = 0.5 17 | self.IMU_az_noise_ = 0.5 18 | 19 | self.IMU_gx_noise_ = 3/180*np.pi # 0.00873 = 0.5 deg/s 20 | self.IMU_gy_noise_ = 3/180*np.pi 21 | self.IMU_gz_noise_ = 3/180*np.pi 22 | self.IMU_mx_noise_ = 50 23 | self.IMU_my_noise_ = 50 24 | self.IMU_mz_noise_ = 50 25 | 26 | self.delay_vicon_step_ = np.ceil(vicon_delay_time_/dt_vicon); 27 | 28 | self.dt_vicon_actual_ = dt_vicon 29 | self.dt_IMU_actual_ = dt_imu 30 | self.reset(states) 31 | 32 | 33 | def reset(self,states): 34 | self.IMU_gx_noise_mean_ = np.random.normal(0,0.02) #0.03 35 | self.IMU_gy_noise_mean_ = np.random.normal(0,0.03) #-0.035 # -0.035 = -1.988deg/s this non zero mean is the drift effect 36 | self.IMU_gz_noise_mean_ = np.random.normal(0,0.015) #0.02 37 | self.last_vicon_update_time_ = 0 38 | self.next_vicon_update_time_ = 0 39 | self.last_IMU_update_time_ = 0 40 | self.next_IMU_update_time_ = 0 41 | self.vicon_roll_cache_ = np.zeros([self.delay_vicon_step_.astype(int)],dtype=np.float64) + states['body_positions'][0,0] 42 | self.vicon_pitch_cache_ = np.zeros([self.delay_vicon_step_.astype(int)],dtype=np.float64) + states['body_positions'][1,0] 43 | self.vicon_yaw_cache_ = np.zeros([self.delay_vicon_step_.astype(int)],dtype=np.float64) + states['body_positions'][2,0] 44 | self.vicon_x_cache_ = np.zeros([self.delay_vicon_step_.astype(int)],dtype=np.float64) + states['body_positions'][3,0] 45 | self.vicon_y_cache_ = np.zeros([self.delay_vicon_step_.astype(int)],dtype=np.float64) + states['body_positions'][4,0] 46 | self.vicon_z_cache_ = np.zeros([self.delay_vicon_step_.astype(int)],dtype=np.float64) + states['body_positions'][5,0] 47 | self.vicon_cache_pointer = 0 48 | self.vicon_update_flag_ = 0 49 | self.vicon_x_old_ = 0 50 | self.vicon_y_old_ = 0 51 | self.vicon_z_old_ = 0 52 | 53 | def update_raw_data(self, time, states): 54 | self.update_Vicon(time, states) 55 | self.update_IMU(time, states) 56 | 57 | def update_Vicon(self, time, states): 58 | if time >= self.next_vicon_update_time_: 59 | self.dt_vicon_actual_ = time - self.last_vicon_update_time_ 60 | 61 | roll_reading = states['body_positions'][0,0] + np.random.normal(0,self.vicon_roll_noise_) 62 | pitch_reading = states['body_positions'][1,0] + np.random.normal(0,self.vicon_pitch_noise_) 63 | yaw_reading = states['body_positions'][2,0] + np.random.normal(0,self.vicon_yaw_noise_) 64 | x_reading = states['body_positions'][3,0] + np.random.normal(0,self.vicon_x_noise_) 65 | y_reading = states['body_positions'][4,0] + np.random.normal(0,self.vicon_y_noise_) 66 | z_reading = states['body_positions'][5,0] + np.random.normal(0,self.vicon_z_noise_) 67 | # roll_reading = states['body_positions'][0,0] 68 | # pitch_reading = states['body_positions'][1,0] 69 | # yaw_reading = states['body_positions'][2,0] 70 | # x_reading = states['body_positions'][3,0] 71 | # y_reading = states['body_positions'][4,0] 72 | # z_reading = states['body_positions'][5,0] 73 | 74 | # cache reading 75 | self.vicon_roll_cache_[self.vicon_cache_pointer] = roll_reading 76 | self.vicon_pitch_cache_[self.vicon_cache_pointer] = pitch_reading 77 | self.vicon_yaw_cache_[self.vicon_cache_pointer] = yaw_reading 78 | self.vicon_x_cache_[self.vicon_cache_pointer] = x_reading 79 | self.vicon_y_cache_[self.vicon_cache_pointer] = y_reading 80 | self.vicon_z_cache_[self.vicon_cache_pointer] = z_reading 81 | 82 | self.vicon_read_pointer = self.vicon_cache_pointer + 1 83 | if self.vicon_read_pointer >= self.delay_vicon_step_: 84 | self.vicon_read_pointer = 0 85 | 86 | # output delayed reading 87 | self.vicon_roll_ = self.vicon_roll_cache_[self.vicon_read_pointer] 88 | self.vicon_pitch_ = self.vicon_pitch_cache_[self.vicon_read_pointer] 89 | self.vicon_yaw_ = self.vicon_yaw_cache_[self.vicon_read_pointer] 90 | self.vicon_x_ = self.vicon_x_cache_[self.vicon_read_pointer] 91 | self.vicon_y_ = self.vicon_y_cache_[self.vicon_read_pointer] 92 | self.vicon_z_ = self.vicon_z_cache_[self.vicon_read_pointer] 93 | 94 | self.vicon_x_dot_ = (self.vicon_x_ - self.vicon_x_old_)/self.dt_vicon_ 95 | self.vicon_y_dot_ = (self.vicon_y_ - self.vicon_y_old_)/self.dt_vicon_ 96 | self.vicon_z_dot_ = (self.vicon_z_ - self.vicon_z_old_)/self.dt_vicon_ 97 | 98 | self.vicon_x_old_ = self.vicon_x_ 99 | self.vicon_y_old_ = self.vicon_y_ 100 | self.vicon_z_old_ = self.vicon_z_ 101 | 102 | self.vicon_cache_pointer = self.vicon_cache_pointer + 1 103 | if self.vicon_cache_pointer >= self.delay_vicon_step_: 104 | self.vicon_cache_pointer = 0 105 | 106 | self.last_vicon_update_time_ = time 107 | self.next_vicon_update_time_ = self.next_vicon_update_time_ + self.dt_vicon_ 108 | 109 | if(time= self.next_IMU_update_time_: 117 | self.dt_IMU_actual_ = time - self.last_IMU_update_time_ 118 | 119 | # self.IMU_ax_ = states['body_accelerations'][3,0] 120 | # self.IMU_ay_ = states['body_accelerations'][4,0] 121 | # self.IMU_az_ = states['body_accelerations'][5,0] 122 | self.IMU_ax_ = states['body_accelerations'][3,0] + np.random.normal(0,self.IMU_ax_noise_) 123 | self.IMU_ay_ = states['body_accelerations'][4,0] + np.random.normal(0,self.IMU_ay_noise_) 124 | self.IMU_az_ = states['body_accelerations'][5,0] + np.random.normal(0,self.IMU_az_noise_) 125 | # self.IMU_gx_ = states['body_velocities'][0,0] 126 | # self.IMU_gy_ = states['body_velocities'][1,0] 127 | # self.IMU_gz_ = states['body_velocities'][2,0] 128 | self.IMU_gx_ = states['body_velocities'][0,0] + np.random.normal(self.IMU_gx_noise_mean_,self.IMU_gx_noise_) 129 | self.IMU_gy_ = states['body_velocities'][1,0] + np.random.normal(self.IMU_gy_noise_mean_,self.IMU_gy_noise_) 130 | self.IMU_gz_ = states['body_velocities'][2,0] + np.random.normal(self.IMU_gz_noise_mean_,self.IMU_gz_noise_) 131 | self.IMU_mx_ = np.random.normal(0,self.IMU_mx_noise_) 132 | self.IMU_my_ = np.random.normal(0,self.IMU_my_noise_) 133 | self.IMU_mz_ = np.random.normal(0,self.IMU_mz_noise_) 134 | 135 | self.last_IMU_update_time_ = time 136 | self.next_IMU_update_time_ = self.next_IMU_update_time_ + self.dt_imu_ 137 | #print('IMU updated!', end="\n\r") 138 | 139 | 140 | 141 | -------------------------------------------------------------------------------- /fwmav_sim_env_maneuver.py: -------------------------------------------------------------------------------- 1 | from rllab.envs.base import Env, Step 2 | from rllab.spaces import Box 3 | import numpy as np 4 | import json 5 | #from simulation_maneuver import Simulation 6 | from Flappy.envs.simulation_maneuver import Simulation 7 | #from pid_xy_arc_z_maneuver import PIDController 8 | from Flappy.envs.controllers.pid_controller import PIDController 9 | 10 | class FWMAVSimEnv(Env): 11 | def __init__(self): 12 | with open ('./Flappy/envs/config/sim_config.json') as file: 13 | sim_config = json.load(file) 14 | with open('./Flappy/envs/config/mav_config_list.json') as file: 15 | mav_config_list = json.load(file) 16 | self.sim = Simulation(mav_config_list, sim_config) 17 | self.action_lb = np.array([0.0, -3, -3.5, -0.15]) 18 | self.action_ub = np.array([18.0, 3, 3.5, 0.15]) 19 | 20 | 21 | @property 22 | def observation_space(self): 23 | return Box(np.array([-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -100.0, -100.0, -100.0]), np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 100.0, 100.0, 100.0])) 24 | 25 | @property 26 | def action_space(self): 27 | return Box(np.array([-5.0, -3, -3.5, -0.15]), np.array([8.0, 3, 3.5, 0.15])) # double the control authority range here so the policy can over ride the controller action 28 | 29 | # def reset(self): 30 | # self.pid_policy = PIDController(self.sim.dt_c) 31 | # rpy_limit = 0.2 # 11.4 deg 32 | # pqr_limit = 0.1 # 5.7 deg/s 33 | # xyz_limit = 0.1 # 10 cm 34 | # xyz_dot_limit = 0.1 # 10 cm/s 35 | 36 | # positions = np.zeros([10,1],dtype=np.float64) 37 | # velocities = np.zeros([10,1],dtype=np.float64) 38 | 39 | # init_attitude = np.random.uniform(-rpy_limit, rpy_limit, size=(3,1)) 40 | # init_angular_velocity = np.random.uniform(-pqr_limit, pqr_limit, size=(3,1)) 41 | # init_position = np.random.uniform(-xyz_limit, xyz_limit, size=(3,1)) 42 | # init_body_velocity = np.random.uniform(-xyz_dot_limit, xyz_dot_limit, size=(3,1)) 43 | 44 | # init_attitude[2] = init_attitude[2]+np.pi 45 | # init_position[0] = init_position[0]-0.35 46 | 47 | # positions[0:3] = init_attitude 48 | # positions[3:6] = init_position 49 | # # positions[5] = 0.0 # initial z 50 | # if positions[3] < -0.35: 51 | # positions[3] = -0.35 # initial x 52 | # velocities[0:6] = np.concatenate((init_angular_velocity,init_body_velocity), axis = 0) 53 | 54 | # self.sim.reset() 55 | # self.sim.set_states(positions, velocities) 56 | # observation = self.sim.observation 57 | 58 | # self.reached = 0 59 | # return observation 60 | 61 | 62 | # # pid controller input 63 | # action_pid = np.squeeze(self.pid_policy.get_action(self.sim.states, self.sim.dt_c, self.reached)) 64 | 65 | # flapper1_states = self.sim.states 66 | 67 | # x = flapper1_states['body_positions'][3] 68 | 69 | # x_dot = flapper1_states['body_spatial_velocities'][0] # spatial x_dot 70 | 71 | # yaw_angle = flapper1_states['body_positions'][2] 72 | 73 | # # combine controller action and policy action 74 | # total_action = np.clip((action + action_pid), self.action_lb, self.action_ub) 75 | 76 | # if self.reached == 0 and x > -0.1 and abs(x_dot) < 1 and abs(yaw_angle) < np.pi/4: 77 | # self.reached = 1 78 | 79 | # if self.reached == 1: 80 | # total_action = np.clip((action_pid), self.action_lb, self.action_ub) 81 | 82 | 83 | # # down sample to 500Hz 84 | # for i in range(20): 85 | # self.sim.step(total_action) 86 | # #self.sim.step(action) 87 | # next_observation = self.sim.observation 88 | # reward = self.sim.reward 89 | # done = self.sim.done 90 | # if (np.mod(self.sim.world.frame,60)==0): 91 | # self.sim.render() 92 | # return Step(observation=next_observation, reward=reward, done=done) 93 | 94 | '''replaced with code from v2/fwmav_sim_env_maneuver.py''' 95 | def reset(self): 96 | self.pid_policy = PIDController(self.sim.dt_c) 97 | rpy_limit = 0.2 # 11.4 deg 98 | pqr_limit = 0.1 # 5.7 deg/s 99 | xyz_limit = 0.05 # 10 cm 100 | xyz_dot_limit = 0.1 # 10 cm/s 101 | 102 | positions = np.zeros([10,1],dtype=np.float64) 103 | velocities = np.zeros([10,1],dtype=np.float64) 104 | 105 | init_attitude = np.random.uniform(-rpy_limit, rpy_limit, size=(3,1)) 106 | init_angular_velocity = np.random.uniform(-pqr_limit, pqr_limit, size=(3,1)) 107 | init_position = np.random.uniform(-xyz_limit, xyz_limit, size=(3,1)) 108 | init_body_velocity = np.random.uniform(-xyz_dot_limit, xyz_dot_limit, size=(3,1)) 109 | 110 | init_attitude[2] = init_attitude[2]+0 111 | init_position[0] = init_position[0]-0 112 | init_position[2] = init_position[2]+0 113 | 114 | positions[0:3] = init_attitude # zhi shi dian 115 | positions[3:6] = init_position 116 | positions[5] = 0.5 # initial z 117 | 118 | # if positions[3] < -0.28: 119 | # positions[3] = -0.28 # initial x 120 | 121 | velocities[0:6] = np.concatenate((init_angular_velocity,init_body_velocity), axis = 0) 122 | 123 | self.sim.reset() 124 | self.sim.set_states(positions, velocities) 125 | observation = self.sim.observation 126 | self.sim.upside_down = 0 #zt 127 | self.sim.reached = 0 128 | return observation 129 | 130 | def step(self, action): 131 | # pid controller input 132 | action_pid = np.squeeze(self.pid_policy.get_action(self.sim.states, self.sim.dt_c, self.sim.reached)) 133 | 134 | flapper1_states = self.sim.states 135 | 136 | # x = flapper1_states['body_positions'][3] 137 | 138 | # x_dot = flapper1_states['body_spatial_velocities'][0] # spatial x_dot 139 | 140 | # yaw_angle = flapper1_states['body_positions'][2] 141 | 142 | # combine controller action and policy action 143 | 144 | action[1] = 0 #zt#switched from 2 to 1 145 | action[3] = 0 146 | 147 | total_action = np.clip((action + action_pid), self.action_lb, self.action_ub) 148 | 149 | # zt 150 | R = self.sim.flapper1.flapper_skel.bodynode('torso').world_transform() 151 | 152 | i_hat = R[0:3,0] #added x project for back flip 153 | k_hat = R[0:3,2] 154 | x_projection = np.dot(i_hat,np.array([1,0,0])) 155 | z_projection = np.dot(k_hat,np.array([0,0,1])) 156 | 157 | roll_vel = flapper1_states['body_velocities'][0] # p 158 | pitch_vel = flapper1_states['body_velocities'][1] # q 159 | #if self.sim.reached == 0 and self.sim.upside_down == 1 and z_projection > 0.8 and abs(roll_vel) < 160: #roll_vel<+-200;currently switched to pitch to train back flip 160 | #self.sim.reach 161 | if self.sim.reached == 0 and x_projection < 0.1 and self.sim.upside_down == 1 and z_projection > 0.8 and abs(pitch_vel) < 160: #roll_vel<+-200;currently switched to pitch to train back flip 162 | self.sim.reached = 1 163 | 164 | 165 | # print("reached = %d" % self.sim.reached, end="\n\r") 166 | # print("upside_down = %d" % self.sim.upside_down, end="\n\r") 167 | # print("z_projection = %d" % z_projection, end="\n\r") 168 | 169 | 170 | if self.sim.reached == 1: 171 | total_action = np.clip((action_pid), self.action_lb, self.action_ub) 172 | 173 | 174 | # down sample to 500Hz 175 | for i in range(20): 176 | self.sim.step(total_action) 177 | #self.sim.step(action) 178 | next_observation = self.sim.observation 179 | reward = self.sim.reward 180 | done = self.sim.done 181 | if (np.mod(self.sim.world.frame,60)==0): 182 | self.sim.render() 183 | return Step(observation=next_observation, reward=reward, done=done) 184 | 185 | def render(self): 186 | self.sim.render() 187 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Flappy 2 | 3 | Flappy Hummingbird: An Open Source Dynamic Simulation of Flapping Wing Robots and Animals 4 | 5 | This work has been published and presented at ICRA2019 (The 2019 International Conference on Robotics and Automation). 6 | 7 | It will be updated continuously. Stay tuned. 8 | 9 | ![](demo.gif) 10 | 11 | ## Publication 12 | 13 | To cite this work in publications: 14 | 15 | @inproceedings{fei2019flappy, 16 | title={Flappy Hummingbird: An Open Source Dynamic Simulation of Flapping Wing Robots and Animals}, 17 | author={Fei, Fan and Tu, Zhan and Yang, Yilun and Zhang, Jian and Deng, Xinyan}, 18 | booktitle={2019 IEEE International Conference on Robotics and Automation (ICRA)}, 19 | pages={9223--9229}, 20 | year={2019}, 21 | organization={IEEE} 22 | } 23 | 24 | ## Getting Started 25 | 26 | These instructions will get you a copy of the project up and running on your local machine for development and testing purposes. See deployment for notes on how to deploy the project on a live system. 27 | 28 | ### Prerequisites 29 | 30 | Flappy requires python3 with the development headers. You'll also need some other system packages. DART is required as a simulation engine (and we use pydart2 as interface). They can be installed as follows 31 | 32 | #### Ubuntu 33 | 34 | # install system packages 35 | sudo apt-get update && sudo apt-get install cmake libopenmpi-dev python3-dev zlib1g-dev swig python3-pip python3-pyqt4 python3- pyqt4.qtopengl 36 | pip install click 37 | 38 | ### Install DART from source 39 | [Install DART from soucrce](https://dartsim.github.io/install_dart_on_ubuntu.html) 40 | * Please install all the dependencies so there will less likely to have errors when installing the repository 41 | * Make sure you install version 6.2.1 by changing 42 | 43 | ```zsh 44 | git checkout tags/v6.8.2 45 | ``` 46 | to 47 | ```zsh 48 | git checkout tags/v6.2.1 49 | ``` 50 | Please refer the [official DART installation document](https://github.com/dartsim/dart/wiki/Installation) when you have problems. 51 | 52 | #### Mac OS X(not finished) 53 | Installation of system packages on Mac requires [Homebrew](https://brew.sh). With Homebrew installed, run the follwing: 54 | ```bash 55 | # install system packages 56 | brew install cmake openmpi 57 | 58 | # install dart 59 | brew install dartsim 60 | ``` 61 | ### Alternative installation methods 62 | 1. Pydart2 source code included in our project. (No extra installation required) 63 | [No installation](https://github.com/chen3082/flappy) 64 | 2. Pydart2 source code not included in our project. (Installation required, which is not a pleasant experience.) 65 | * Please continue your steps here 66 | 3. Docker (If you do not have access to recommanded OS environment) 67 | [Docker container](https://github.com/chen3082/docker) 68 | 69 | ### Virtual environment 70 | From the general python package sanity perspective, it is a good idea to use virtual environments (virtualenvs) to make sure packages from different projects do not interfere with each other. Creation of virtual environments is done by executing the command [venv](https://docs.python.org/3/library/venv.html#module-venv): 71 | 72 | ```zsh 73 | python3 -m venv /path/to/new/virtual/environment 74 | ``` 75 | 76 | To activate a venv: 77 | 78 | ```zsh 79 | source /path/to/venv/bin/activate 80 | ``` 81 | 82 | ### Installation 83 | - Pydart2 is a python binding to DART. 84 | 85 | ```zsh 86 | pip install pydart2 87 | ``` 88 | If this does not work please try to install from source. 89 | Please refer to [document](https://pydart2.readthedocs.io/en/latest/install.html) when you have problems. 90 | 91 | - [Tensorflow](https://github.com/tensorflow/tensorflow) is needed for the usage of neural network. If you want to make use of your GPU, please install the tensorflow with gpu 92 | - Please make sure you install tensorflow version < 2.0 93 | 94 | ```zsh 95 | pip install tensorflow-gpu # if you have a CUDA-compatible gpu and proper drivers 96 | ``` 97 | else 98 | ```zsh 99 | pip install tensorflow==1.9 100 | ``` 101 | please refer to [TensorFlow installation guide](https://www.tensorflow.org/install/) 102 | for more details. 103 | 104 | - Clone the repo and cd into it: 105 | ```zsh 106 | git clone https://github.com/purdue-biorobotics/flappy.git 107 | cd flappy 108 | ``` 109 | 110 | - Install Flappy package 111 | ```zsh 112 | pip install -e . 113 | ``` 114 | 115 | - Lack dependency 116 | * Please try to install any dependency if there is an error related to that library during the installation process. 117 | ```zhs 118 | pip [Dependency lack of] 119 | or 120 | apt-get install [Dependency lack of] 121 | ``` 122 | 123 | ## Environments 124 | ### FWMAV 125 | Dual motor driven flapping wing robots based on the Purdue Hummingbird robot. 126 | 127 | The control of this vehicle is a difficult problem. We challenge developers, researchers, scientists and roboticists to come up with better control algorithms, either feedback controller or learning-based controller. 128 | 129 | Two default working controller are included: a cascading PID controller (control structure similar to ArduPilot) and an Adaptive Robust Controller (ARC). These two controllers can be evaluated in the provided test script. 130 | 131 | #### 'fwmav_hover-v0' 132 | This environment is for controlling the dual wing flappin wing robot. 133 | 134 | The four inputs are the thrust and torque signals: 135 | * voltage_amplitude_ [0,18] volt for thrust 136 | * differential_voltage_ [-3,3] volt for roll torque 137 | * mean_voltage_ [-3.5,3.5] volt for pitch torque 138 | * split_cycle_ [-0.15,0.15] for yaw torque 139 | 140 | This mode of control is similar to helicopter or quadcopter control. The sinusoidal voltage signal drives the wing back and forth is generated by using [wing beat modulation](https://arc.aiaa.org/doi/10.2514/1.47146). 141 | 142 | The input actions are in [-1,1] and will be scaled to their appropriate range. If implementing a feedback controller, the input should be scaled to [-1,1]. See the baseline PID controller and test example for detail. 143 | 144 | ##### Testing with Closed loop controller (PID or ARC) 145 | ```zsh 146 | python test.py --model_type=PID 147 | python test.py --model_type=ARC 148 | ``` 149 | 150 | ##### Learning 151 | We choose to use stable baselines instead of baselines as our RL library. Note that our environment still follows the specification of the gym/env, so baselines can be applied to our env as well. 152 | 153 | ###### Training 154 | ```zsh 155 | python train.py --model_type=PPO2 --model_path=ppo2_mlp --policy_type=MlpPolicy --n_cpu=12 --time_step=100000 156 | ``` 157 | 158 | ###### Testing with trained model 159 | ```zsh 160 | python test.py --model_type=PPO2 --model_path=ppo2_mlp --policy_type=MlpPolicy 161 | ``` 162 | 163 | #### 'fwmav_hover-v1' 164 | This environment similar to `'fwmav_hover-v0'`, but without using [wing beat modulation][doman2010wingbeat]. 165 | 166 | An example using PID is in `test_simple.py`, this example still uses the same PID controller and wing beat modulation. But the input to the environment are just two voltage signals ([-18,18] volt) supplied to the two motors. 167 | 168 | To develop a new control policy, the policy should generate sinusoidal signals near 34Hz to drive the wings and implement control at the same time. Without specifying the wing kinematics allows the ability to generate torque and force in arbitrary directions. 169 | 170 | ##### Testing with Closed loop controller (PID or ARC) 171 | ```zsh 172 | python test_simple.py --model_type=PID 173 | python test_simple.py --model_type=ARC 174 | ``` 175 | 176 | 177 | ## Contributor 178 | Fan Fei, Ruoyu Wu, Jian Zhang, Zhan Tu, Yunlei Yan, Yuan-Cheng Chen 179 | 180 | ## License 181 | MIT 182 | 183 | ## Acknowledments 184 | 185 | -------------------------------------------------------------------------------- /fwmav_sim_env.py: -------------------------------------------------------------------------------- 1 | ########################## FWMAV Simulation ######################### 2 | # Version 0.3 3 | # Fan Fei Feb 2019 4 | # Direct motor driven flapping wing MAV simulation 5 | ####################################################################### 6 | 7 | 8 | import gym 9 | from gym.spaces import Box 10 | import numpy as np 11 | import json 12 | 13 | from Flappy.envs.simulation import Simulation 14 | from Flappy.envs.controllers.pid_controller import PIDController 15 | from Flappy.envs.mission import Mission 16 | 17 | class FWMAVSimEnv(gym.Env): 18 | def __init__(self): 19 | with open ('./Flappy/envs/config/sim_config.json') as file: 20 | sim_config = json.load(file) 21 | with open('./Flappy/envs/config/mav_config_list.json') as file: 22 | mav_config_list = json.load(file) 23 | self.sim = Simulation(mav_config_list, sim_config) 24 | self.observation = np.zeros([18], dtype=np.float64) 25 | self.observation_bound = np.array([ 26 | 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 27 | 1.0, 1.0, 1.0, 1.0, 1.0 28 | ]) 29 | 30 | self.action_lb = np.array([-5, -3, -3.5, -0.15]) 31 | self.action_ub = np.array([11, 3, 3.5, 0.15]) 32 | self.total_action_lb = np.array([0, -3, -3.5, -0.15]) 33 | self.total_action_ub = np.array([18.0, 3, 3.5, 0.15]) 34 | self.action_old = np.array([0, 0, 0, 0]) 35 | self.pid_policy = PIDController(self.sim.dt_c) 36 | self.mission = Mission(self.pid_policy) 37 | 38 | self.done = False 39 | self.reward = 0 40 | 41 | self.is_print = False 42 | 43 | def enable_viz(self): 44 | self.sim.enable_viz() 45 | 46 | def enable_print(self): 47 | self.is_print = True 48 | 49 | def set_attack_type(self, attack_type): 50 | self.attack_type = attack_type 51 | 52 | @property 53 | def observation_space(self): 54 | return Box(np.array([-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf]), np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf])) 55 | 56 | @property 57 | def action_space(self): 58 | return Box(np.array([-5.0, -3, -3.5, -0.15]), np.array([11.0, 3, 3.5, 0.15])) 59 | 60 | def reset(self): 61 | rpy_limit = 0.785398 # 45 deg 62 | pqr_limit = 3.14159 # 180 deg/s 63 | xyz_limit = 0.1 # 10 cm 64 | xyz_dot_limit = 0.1 # 10cm/s 65 | 66 | positions = np.zeros([10,1], dtype=np.float64) 67 | velocities = np.zeros([10,1], dtype=np.float64) 68 | 69 | init_attitude = np.random.uniform(-rpy_limit, rpy_limit, size=(3,1)) 70 | init_angular_velocity = np.random.uniform(-pqr_limit, pqr_limit, size=(3,1)) 71 | init_position = np.random.uniform(-xyz_limit, xyz_limit, size=(3, 1)) 72 | init_body_velocity = np.random.uniform(-xyz_dot_limit, xyz_dot_limit, size=(3,1)) 73 | 74 | positions[0:3] = init_attitude 75 | positions[3:6] = init_position 76 | velocities[0:6] = np.concatenate((init_angular_velocity,init_body_velocity), axis = 0) 77 | 78 | self.sim.reset() 79 | self.sim.set_states(positions, velocities) 80 | self.observation = self.get_observation() 81 | self.next_control_time = self.sim.dt_c 82 | self.mission.reset() 83 | 84 | return self.observation 85 | 86 | def seed(self, seed=0): 87 | # np.random.seed(seed) 88 | return 89 | 90 | def step(self, action): 91 | # scale action from [-1,1] to [action_lb, action_ub] 92 | scaled_action = self.action_lb + \ 93 | (action + 1.) * 0.5 * (self.action_ub - self.action_lb) 94 | scaled_action = np.clip(scaled_action, self.action_lb, self.action_ub) 95 | 96 | self.mission.update_waypoint(self.sim.world.time()) 97 | 98 | action_pid = np.squeeze( 99 | self.pid_policy.get_action(self.sim.states, self.sim.dt_c, 100 | self.sim.sensor_fusion)) 101 | 102 | # input_signal = policy_action + pid_action 103 | input_signal = np.clip(scaled_action + action_pid, self.total_action_lb, self.total_action_ub) 104 | 105 | # run simulation and down sample from f_sim to f_control 106 | while self.sim.world.time() < self.next_control_time: 107 | self.sim.step(input_signal) 108 | self.next_control_time += self.sim.dt_c 109 | 110 | self.observation = self.get_observation() 111 | reward = self.get_reward(action) 112 | done = self.get_terminal() 113 | 114 | return self.observation, reward, done, {} 115 | 116 | def get_observation(self): 117 | # define observations here3 118 | # 119 | # observations are the following 120 | # rotation matrix 121 | # positions 122 | # linear velocities 123 | # angular velocities 124 | 125 | observation = np.zeros([18],dtype=np.float64) 126 | # get full states 127 | flapper1_states = self.sim.states 128 | # create rotation matrix 129 | roll_angle = flapper1_states['body_positions'][0] 130 | pitch_angle = flapper1_states['body_positions'][1] 131 | yaw_angle = flapper1_states['body_positions'][2] 132 | 133 | R = self.euler_2_R(roll_angle, pitch_angle, yaw_angle) 134 | observation[0:9] = R.reshape(-1) 135 | 136 | # other states 137 | observation[9] = flapper1_states['body_positions'][3] - self.mission.pos_target_x_ # special x 138 | observation[10] = flapper1_states['body_positions'][4] - self.mission.pos_target_y_ # special y 139 | observation[11] = flapper1_states['body_positions'][5] - self.mission.pos_target_z_ # special z 140 | observation[12] = flapper1_states['body_spatial_velocities'][0] # spatial x_dot 141 | observation[13] = flapper1_states['body_spatial_velocities'][1] # spatial y_dot 142 | observation[14] = flapper1_states['body_spatial_velocities'][2] # spatial z_dot 143 | observation[15] = flapper1_states['body_velocities'][0] # p 144 | observation[16] = flapper1_states['body_velocities'][1] # q 145 | observation[17] = flapper1_states['body_velocities'][2] # r 146 | 147 | observation = observation/self.observation_bound 148 | 149 | return observation 150 | 151 | def get_reward(self, action): 152 | reward = 0 153 | 154 | flapper1_states = self.sim.states 155 | 156 | position = flapper1_states['body_positions'][3:6] 157 | position_target = np.array([self.mission.pos_target_x_,self.mission.pos_target_y_,self.mission.pos_target_z_]) 158 | position_error = position_target - position 159 | 160 | angular_position = flapper1_states['body_positions'][0:3] 161 | angular_position_target = np.array([[0.0], [0.0], [0.0]]) 162 | angular_position_error = angular_position_target - angular_position 163 | 164 | linear_velocity = flapper1_states['body_spatial_velocities'] 165 | angular_velocity = flapper1_states['body_velocities'][0:3] 166 | 167 | d_action = action - self.action_old 168 | self.action_old = action 169 | 170 | control_cost = 2e-4*np.sum(np.square(action)) 171 | d_control_cost = 2*np.sum(np.square(d_action)) 172 | 173 | position_cost = 4e-3*np.linalg.norm(position_error) 174 | angular_position_cost = 8e-3*np.linalg.norm(angular_position_error) 175 | velocity_cots = 5e-4*np.linalg.norm(linear_velocity) 176 | angular_velocity_cost = 6e-4*np.linalg.norm(angular_velocity) 177 | 178 | stability_cost = position_cost + angular_position_cost + velocity_cots + angular_velocity_cost 179 | 180 | cost = stability_cost + control_cost + d_control_cost 181 | reward = 1 / (cost**2 + 1e-6) 182 | 183 | return reward 184 | 185 | def get_terminal(self): 186 | if self.sim.check_collision(): 187 | return True 188 | if self.sim.world.time() > 20: 189 | return True 190 | 191 | flapper1_states = self.sim.states 192 | x = flapper1_states['body_positions'][3] 193 | y = flapper1_states['body_positions'][4] 194 | z = flapper1_states['body_positions'][5] 195 | 196 | distance = (x**2 + y**2 + z**2)**0.5 197 | if distance > 1: 198 | return True 199 | return False 200 | 201 | def euler_2_R(self, phi, theta, psi): 202 | R = np.zeros([3, 3], dtype=np.float64) 203 | 204 | C_phi = np.cos(phi) 205 | S_phi = np.sin(phi) 206 | C_theta = np.cos(theta) 207 | S_theta = np.sin(theta) 208 | C_psi = np.cos(psi) 209 | S_psi = np.sin(psi) 210 | 211 | R[0, 0] = C_psi * C_theta 212 | R[0, 1] = C_psi * S_theta * C_phi - S_psi * C_phi 213 | R[0, 2] = C_psi * S_theta * C_phi + S_psi * S_phi 214 | R[1, 0] = S_psi * C_theta 215 | R[1, 1] = S_psi * S_theta * S_phi + C_psi * C_phi 216 | R[1, 2] = S_psi * S_theta * C_phi - C_psi * S_phi 217 | R[2, 0] = -S_theta 218 | R[2, 1] = C_theta * S_phi 219 | R[2, 2] = C_theta * C_phi 220 | 221 | return R 222 | 223 | def render(self): 224 | # self.sim.render() 225 | return 226 | -------------------------------------------------------------------------------- /flappy/envs/fwmav/fwmav_sim_en_old_with_control_working.py: -------------------------------------------------------------------------------- 1 | ########################## FWMAV Simulation ######################### 2 | # Version 0.3 3 | # Fan Fei Feb 2019 4 | # Direct motor driven flapping wing MAV simulation 5 | ####################################################################### 6 | 7 | import gym 8 | from gym.spaces import Box 9 | import numpy as np 10 | import json 11 | 12 | from flappy.envs.fwmav.simulation import Simulation 13 | # from flappy.envs.controllers.pid_controller import PIDController 14 | from flappy.envs.fwmav.controllers.arc_xy_arc_z import PIDController 15 | from flappy.envs.fwmav.mission import Mission 16 | 17 | # GUI 18 | from flappy.envs.fwmav.MyGLUTWindow import GUI 19 | import time 20 | 21 | class FWMAVSimEnv(gym.Env): 22 | def __init__(self): 23 | with open ('./flappy/envs/fwmav/config/sim_config.json') as file: 24 | sim_config = json.load(file) 25 | with open('./flappy/envs/fwmav/config/mav_config_list.json') as file: 26 | mav_config_list = json.load(file) 27 | self.sim = Simulation(mav_config_list, sim_config) 28 | self.observation = np.zeros([18], dtype=np.float64) 29 | self.observation_bound = np.array([ 30 | 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 31 | 1.0, 1.0, 1.0, 1.0, 1.0 32 | ]) 33 | 34 | self.action_lb = np.array([-5, -3, -3.5, -0.15]) 35 | self.action_ub = np.array([11, 3, 3.5, 0.15]) 36 | self.total_action_lb = np.array([0, -3, -3.5, -0.15]) 37 | self.total_action_ub = np.array([18.0, 3, 3.5, 0.15]) 38 | self.action_old = np.array([0, 0, 0, 0]) 39 | self.pid_policy = PIDController(self.sim.dt_c) 40 | self.mission = Mission(self.pid_policy) 41 | 42 | self.done = False 43 | self.reward = 0 44 | 45 | self.is_sim_on = False 46 | self.is_print_on = False 47 | self.is_visual_on = False 48 | self.dt_v = 1/sim_config['f_visual'] 49 | 50 | def enable_visualization(self): 51 | self.is_visual_on = True 52 | self.next_visualization_time = time.time() 53 | 54 | self.gui = GUI(self, "FWMAV") 55 | self.gui.cv.acquire() 56 | print("init GUI") 57 | self.gui.start() 58 | 59 | def enable_print(self): 60 | self.is_print_on = True 61 | 62 | @property 63 | def observation_space(self): 64 | return Box(np.array([-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf]), np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf])) 65 | 66 | @property 67 | def action_space(self): 68 | return Box(np.array([-5.0, -3, -3.5, -0.15]), np.array([11.0, 3, 3.5, 0.15])) 69 | 70 | def reset(self): 71 | # rpy_limit = 0.785398 # 45 deg 72 | # pqr_limit = 3.14159 # 180 deg/s 73 | # xyz_limit = 0.1 # 10 cm 74 | # xyz_dot_limit = 0.1 # 10cm/s 75 | rpy_limit = 0.0 # 45 deg 76 | pqr_limit = 0.0 # 180 deg/s 77 | xyz_limit = 0.0 # 10 cm 78 | xyz_dot_limit = 0.0 # 10cm/s 79 | 80 | positions = np.zeros([10,1], dtype=np.float64) 81 | velocities = np.zeros([10,1], dtype=np.float64) 82 | 83 | init_attitude = np.random.uniform(-rpy_limit, rpy_limit, size=(3,1)) 84 | init_angular_velocity = np.random.uniform(-pqr_limit, pqr_limit, size=(3,1)) 85 | init_position = np.random.uniform(-xyz_limit, xyz_limit, size=(3, 1)) 86 | init_body_velocity = np.random.uniform(-xyz_dot_limit, xyz_dot_limit, size=(3,1)) 87 | 88 | positions[0:3] = init_attitude 89 | positions[3:6] = init_position 90 | velocities[0:6] = np.concatenate((init_angular_velocity,init_body_velocity), axis = 0) 91 | 92 | self.sim.reset() 93 | self.sim.set_states(positions, velocities) 94 | self.observation = self.get_observation() 95 | self.next_control_time = self.sim.dt_c 96 | self.mission.reset() 97 | self.is_sim_on = False 98 | self.next_visualization_time = time.time() 99 | 100 | return self.observation 101 | 102 | def seed(self, seed=0): 103 | # np.random.seed(seed) 104 | return 105 | 106 | def step(self, action): 107 | # scale action from [-1,1] to [action_lb, action_ub] 108 | scaled_action = self.action_lb + \ 109 | (action + 1.) * 0.5 * (self.action_ub - self.action_lb) 110 | scaled_action = np.clip(scaled_action, self.action_lb, self.action_ub) 111 | 112 | self.mission.update_waypoint(self.sim.world.time()) 113 | 114 | action_pid = np.squeeze( 115 | self.pid_policy.get_action(self.sim.states, self.sim.dt_c, 116 | self.sim.sensor_fusion)) 117 | 118 | # input_signal = policy_action + pid_action 119 | input_signal = np.clip(scaled_action + action_pid, self.total_action_lb, self.total_action_ub) 120 | 121 | # run simulation and down sample from f_sim to f_control 122 | while self.sim.world.time() < self.next_control_time: 123 | self.sim.step(input_signal) 124 | self.next_control_time += self.sim.dt_c 125 | 126 | self.observation = self.get_observation() 127 | reward = self.get_reward(action) 128 | done = self.get_terminal() 129 | 130 | # visulization 131 | if self.is_visual_on and time.time() >= self.next_visualization_time: 132 | self.gui.cv.wait() 133 | self.next_visualization_time = time.time() + self.dt_v 134 | 135 | info = {'time' : self.sim.world.time()} 136 | return self.observation, reward, done, info 137 | 138 | def get_observation(self): 139 | # define observations here3 140 | # 141 | # observations are the following 142 | # rotation matrix 143 | # positions 144 | # linear velocities 145 | # angular velocities 146 | 147 | observation = np.zeros([18],dtype=np.float64) 148 | # get full states 149 | flapper1_states = self.sim.states 150 | # create rotation matrix 151 | roll_angle = flapper1_states['body_positions'][0] 152 | pitch_angle = flapper1_states['body_positions'][1] 153 | yaw_angle = flapper1_states['body_positions'][2] 154 | 155 | R = self.euler_2_R(roll_angle, pitch_angle, yaw_angle) 156 | observation[0:9] = R.reshape(-1) 157 | 158 | # other states 159 | observation[9] = flapper1_states['body_positions'][3] - self.mission.pos_target_x_ # special x 160 | observation[10] = flapper1_states['body_positions'][4] - self.mission.pos_target_y_ # special y 161 | observation[11] = flapper1_states['body_positions'][5] - self.mission.pos_target_z_ # special z 162 | observation[12] = flapper1_states['body_spatial_velocities'][0] # spatial x_dot 163 | observation[13] = flapper1_states['body_spatial_velocities'][1] # spatial y_dot 164 | observation[14] = flapper1_states['body_spatial_velocities'][2] # spatial z_dot 165 | observation[15] = flapper1_states['body_velocities'][0] # p 166 | observation[16] = flapper1_states['body_velocities'][1] # q 167 | observation[17] = flapper1_states['body_velocities'][2] # r 168 | 169 | observation = observation/self.observation_bound 170 | 171 | return observation 172 | 173 | def get_reward(self, action): 174 | reward = 0 175 | 176 | flapper1_states = self.sim.states 177 | 178 | position = flapper1_states['body_positions'][3:6] 179 | position_target = np.array([self.mission.pos_target_x_,self.mission.pos_target_y_,self.mission.pos_target_z_]) 180 | position_error = position_target - position 181 | 182 | angular_position = flapper1_states['body_positions'][0:3] 183 | angular_position_target = np.array([[0.0], [0.0], [0.0]]) 184 | angular_position_error = angular_position_target - angular_position 185 | 186 | linear_velocity = flapper1_states['body_spatial_velocities'] 187 | angular_velocity = flapper1_states['body_velocities'][0:3] 188 | 189 | d_action = action - self.action_old 190 | self.action_old = action 191 | 192 | control_cost = 2e-4*np.sum(np.square(action)) 193 | d_control_cost = 2*np.sum(np.square(d_action)) 194 | 195 | position_cost = 4e-3*np.linalg.norm(position_error) 196 | angular_position_cost = 8e-3*np.linalg.norm(angular_position_error) 197 | velocity_cots = 5e-4*np.linalg.norm(linear_velocity) 198 | angular_velocity_cost = 6e-4*np.linalg.norm(angular_velocity) 199 | 200 | stability_cost = position_cost + angular_position_cost + velocity_cots + angular_velocity_cost 201 | 202 | cost = stability_cost + control_cost + d_control_cost 203 | reward = 1 / (cost**2 + 1e-6) 204 | 205 | return reward 206 | 207 | def get_terminal(self): 208 | if self.sim.check_collision(): 209 | return True 210 | if self.sim.world.time() > 20: 211 | return True 212 | 213 | flapper1_states = self.sim.states 214 | x = flapper1_states['body_positions'][3] 215 | y = flapper1_states['body_positions'][4] 216 | z = flapper1_states['body_positions'][5] 217 | 218 | distance = (x**2 + y**2 + z**2)**0.5 219 | if distance > 1: 220 | return True 221 | return False 222 | 223 | def euler_2_R(self, phi, theta, psi): 224 | R = np.zeros([3, 3], dtype=np.float64) 225 | 226 | C_phi = np.cos(phi) 227 | S_phi = np.sin(phi) 228 | C_theta = np.cos(theta) 229 | S_theta = np.sin(theta) 230 | C_psi = np.cos(psi) 231 | S_psi = np.sin(psi) 232 | 233 | R[0, 0] = C_psi * C_theta 234 | R[0, 1] = C_psi * S_theta * C_phi - S_psi * C_phi 235 | R[0, 2] = C_psi * S_theta * C_phi + S_psi * S_phi 236 | R[1, 0] = S_psi * C_theta 237 | R[1, 1] = S_psi * S_theta * S_phi + C_psi * C_phi 238 | R[1, 2] = S_psi * S_theta * C_phi - C_psi * S_phi 239 | R[2, 0] = -S_theta 240 | R[2, 1] = C_theta * S_phi 241 | R[2, 2] = C_theta * C_phi 242 | 243 | return R 244 | 245 | -------------------------------------------------------------------------------- /flappy/envs/fwmav/fwmav_sim_env.py: -------------------------------------------------------------------------------- 1 | ########################## FWMAV Simulation ######################### 2 | # Version 0.3 3 | # Fan Fei Feb 2019 4 | # Direct motor driven flapping wing MAV simulation 5 | ####################################################################### 6 | 7 | import gym 8 | from gym.spaces import Box 9 | import numpy as np 10 | import json 11 | import pydart2 as pydart 12 | 13 | from flappy.envs.fwmav.simulation import Simulation 14 | from flappy.envs.fwmav.mission import Mission 15 | 16 | # GUI 17 | from flappy.envs.fwmav.MyGLUTWindow import GUI 18 | import time 19 | 20 | class FWMAVSimEnv(gym.Env): 21 | def __init__(self): 22 | # initialize simulation and fwmav 23 | with open ('./flappy/envs/fwmav/config/sim_config.json') as file: 24 | sim_config = json.load(file) 25 | with open('./flappy/envs/fwmav/config/mav_config_list.json') as file: 26 | mav_config_list = json.load(file) 27 | self.sim = Simulation(mav_config_list, sim_config) 28 | # ground_skel = self.sim.world.add_skeleton('./flappy/urdf/ground.urdf') 29 | 30 | self.observation = np.zeros([18], dtype=np.float64) 31 | self.observation_bound = np.array([ 32 | 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 33 | 1.0, 1.0, 1.0, 1.0, 1.0 34 | ]) 35 | 36 | self.action_lb = np.array([0.0, -3.0, -3.5, -0.15]) 37 | self.action_ub = np.array([18.0, 3.0, 3.5, 0.15]) 38 | self.total_action_lb = np.array([0.0, -3.0, -3.5, -0.15]) 39 | self.total_action_ub = np.array([18.0, 3.0, 3.5, 0.15]) 40 | self.action_old = np.array([0, 0, 0, 0]) 41 | self.mission = Mission() 42 | 43 | self.done = False 44 | self.reward = 0 45 | self.random_init = True 46 | 47 | self.is_sim_on = False 48 | self.is_print_on = False 49 | self.is_visual_on = False 50 | self.dt_v = 1/sim_config['f_visual'] 51 | 52 | def config(self, random_init=True, randomize_sim=True, phantom_sensor=False): 53 | self.random_init = random_init 54 | self.sim.randomize = randomize_sim 55 | self.sim.phantom_sensor = phantom_sensor 56 | self.sim.sensor_fusion.phantom = phantom_sensor 57 | if randomize_sim == False: 58 | self.sim.flapper1.nominal() 59 | 60 | def enable_visualization(self): 61 | self.is_visual_on = True 62 | self.next_visualization_time = time.time() 63 | 64 | self.gui = GUI(self, "FWMAV") 65 | self.gui.cv.acquire() 66 | print("init GUI") 67 | self.gui.start() 68 | 69 | def enable_print(self): 70 | self.is_print_on = True 71 | 72 | @property 73 | def observation_space(self): 74 | return Box(np.array([-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf]), np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf])) 75 | 76 | @property 77 | def action_space(self): 78 | return Box(np.array([-1, -1, -1, -1]), np.array([1, 1, 1, 1])) 79 | 80 | def reset(self): 81 | if self.random_init: 82 | rpy_limit = 0.785398 # 45 deg 83 | pqr_limit = 3.14159 # 180 deg/s 84 | xyz_limit = 0.2 # 20 cm 85 | xyz_dot_limit = 0.2 # 20 cm/s 86 | else: 87 | rpy_limit = 0.0 88 | pqr_limit = 0.0 89 | xyz_limit = 0.0 90 | xyz_dot_limit = 0.0 91 | 92 | positions = np.zeros([10,1], dtype=np.float64) 93 | velocities = np.zeros([10,1], dtype=np.float64) 94 | 95 | init_attitude = np.random.uniform(-rpy_limit, rpy_limit, size=(3,1)) 96 | init_angular_velocity = np.random.uniform(-pqr_limit, pqr_limit, size=(3,1)) 97 | init_position = np.random.uniform(-xyz_limit, xyz_limit, size=(3, 1)) 98 | init_body_velocity = np.random.uniform(-xyz_dot_limit, xyz_dot_limit, size=(3,1)) 99 | 100 | positions[0:3] = init_attitude 101 | positions[3:6] = init_position 102 | velocities[0:6] = np.concatenate((init_angular_velocity,init_body_velocity), axis = 0) 103 | 104 | self.sim.reset() 105 | self.sim.set_states(positions, velocities) 106 | self.observation = self.get_observation() 107 | self.next_control_time = self.sim.dt_c 108 | self.mission.reset() 109 | self.is_sim_on = False 110 | self.next_visualization_time = time.time() 111 | 112 | return self.observation 113 | 114 | def seed(self, seed=0): 115 | # np.random.seed(seed) 116 | return 117 | 118 | def step(self, action): 119 | # scale action from [-1,1] to [action_lb, action_ub] 120 | # since baseline does not support asymmetric action space 121 | scaled_action = (action+1)*0.5*(self.action_ub-self.action_lb)+self.action_lb 122 | scaled_action = np.clip(scaled_action, self.action_lb, self.action_ub) 123 | 124 | total_action = np.clip(scaled_action, self.total_action_lb, self.total_action_ub) 125 | 126 | # convert action to voltage signal 127 | max_voltage = total_action[0] 128 | voltage_diff = total_action[1] 129 | voltage_bias = total_action[2] 130 | split_cycle = total_action[3] 131 | # max_voltage = action[0] 132 | # voltage_diff = action[1] 133 | # voltage_bias = action[2] 134 | # split_cycle = action[3] 135 | input_voltage = np.zeros([2],dtype=np.float64) 136 | input_voltage[0] = self.generate_control_signal(self.sim.flapper1.frequency, max_voltage, voltage_diff, voltage_bias, -split_cycle, self.sim.world.time(), 0) 137 | input_voltage[1] = self.generate_control_signal(self.sim.flapper1.frequency, max_voltage, -voltage_diff, voltage_bias, split_cycle, self.sim.world.time(), 0) 138 | input_voltage[0] = np.clip(input_voltage[0], -18, 18) 139 | input_voltage[1] = np.clip(input_voltage[1], -18, 18) 140 | 141 | # run simulation and down sample from f_sim to f_control 142 | while self.sim.world.time() < self.next_control_time: 143 | self.sim.step(input_voltage) 144 | self.next_control_time += self.sim.dt_c 145 | 146 | # update control target 147 | self.mission.update_waypoint(self.sim.world.time()) 148 | 149 | # update observation, reward, terminal 150 | self.observation = self.get_observation() 151 | reward = self.get_reward(action) 152 | done = self.get_terminal() 153 | 154 | # visulization 155 | if self.is_visual_on and time.time() >= self.next_visualization_time: 156 | self.gui.cv.wait() 157 | self.next_visualization_time = time.time() + self.dt_v 158 | 159 | info = {'time' : self.sim.world.time()} 160 | return self.observation, reward, done, info 161 | 162 | def get_observation(self): 163 | # define observations here3 164 | # 165 | # observations are the following 166 | # rotation matrix 167 | # positions 168 | # linear velocities 169 | # angular velocities 170 | 171 | observation = np.zeros([18],dtype=np.float64) 172 | 173 | # get full states 174 | roll_angle = self.sim.sensor_fusion.out_roll_ 175 | pitch_angle = self.sim.sensor_fusion.out_pitch_ 176 | yaw_angle = self.sim.sensor_fusion.out_yaw_ 177 | 178 | R = self.euler_2_R(roll_angle, pitch_angle, yaw_angle) 179 | observation[0:9] = R.reshape(-1) 180 | 181 | observation[9] = self.sim.sensor_fusion.out_x_ - self.mission.pos_target_x_ 182 | observation[10] = self.sim.sensor_fusion.out_y_ - self.mission.pos_target_y_ 183 | observation[11] = self.sim.sensor_fusion.out_z_ - self.mission.pos_target_z_ 184 | observation[12] = self.sim.sensor_fusion.out_x_dot_ 185 | observation[13] = self.sim.sensor_fusion.out_y_dot_ 186 | observation[14] = self.sim.sensor_fusion.out_z_dot_ 187 | observation[15] = self.sim.sensor_fusion.IMU_gx_ 188 | observation[16] = self.sim.sensor_fusion.IMU_gy_ 189 | observation[17] = self.sim.sensor_fusion.IMU_gz_ 190 | 191 | observation = observation/self.observation_bound 192 | 193 | return observation 194 | 195 | def get_reward(self, action): 196 | reward = 0 197 | 198 | flapper1_states = self.sim.states 199 | 200 | position = flapper1_states['body_positions'][3:6] 201 | position_target = np.array([self.mission.pos_target_x_,self.mission.pos_target_y_,self.mission.pos_target_z_]) 202 | position_error = position_target - position 203 | 204 | angular_position = flapper1_states['body_positions'][0:3] 205 | angular_position_target = np.array([[0.0], [0.0], [0.0]]) 206 | angular_position_error = angular_position_target - angular_position 207 | 208 | linear_velocity = flapper1_states['body_spatial_velocities'] 209 | angular_velocity = flapper1_states['body_velocities'][0:3] 210 | 211 | d_action = action - self.action_old 212 | self.action_old = action 213 | 214 | control_cost = 2e-4*np.sum(np.square(action)) 215 | d_control_cost = 2*np.sum(np.square(d_action)) 216 | 217 | position_cost = 4e-3*np.linalg.norm(position_error) 218 | angular_position_cost = 8e-3*np.linalg.norm(angular_position_error) 219 | velocity_cots = 5e-4*np.linalg.norm(linear_velocity) 220 | angular_velocity_cost = 6e-4*np.linalg.norm(angular_velocity) 221 | 222 | stability_cost = position_cost + angular_position_cost + velocity_cots + angular_velocity_cost 223 | 224 | cost = stability_cost + control_cost + d_control_cost 225 | reward = 1 / (cost**2 + 1e-6) 226 | 227 | return reward 228 | 229 | def get_terminal(self): 230 | if self.sim.check_collision(): 231 | return True 232 | if self.sim.world.time() > 2: 233 | return True 234 | 235 | flapper1_states = self.sim.states 236 | x = flapper1_states['body_positions'][3] 237 | y = flapper1_states['body_positions'][4] 238 | z = flapper1_states['body_positions'][5] 239 | 240 | distance = (x**2 + y**2 + z**2)**0.5 241 | if distance > 1: 242 | return True 243 | return False 244 | 245 | def generate_control_signal(self, f, Umax, delta, bias, sc, t, phase_0): 246 | V = Umax + delta 247 | V0 = bias 248 | sigma = 0.5+sc 249 | 250 | T = 1/f 251 | t_phase = phase_0/360*T 252 | t = t+t_phase 253 | period = np.floor(t/T) 254 | t = t-period*T 255 | 256 | if 0<=t and t -self.x_t+0.03: 114 | positions[3] = -self.x_t+0.03 # initial x 115 | velocities[0:6] = np.concatenate((init_angular_velocity,init_body_velocity), axis = 0) 116 | 117 | self.sim.reset() 118 | self.sim.set_states(positions, velocities) 119 | self.observation = self.get_observation() 120 | self.next_control_time = self.sim.dt_c 121 | self.mission.reset() 122 | self.is_sim_on = False 123 | self.next_visualization_time = time.time() 124 | 125 | self.reached = False 126 | 127 | return self.observation 128 | 129 | def seed(self, seed=0): 130 | # np.random.seed(seed) 131 | return 132 | 133 | def step(self, action): 134 | # scale action from [-1,1] to [action_lb, action_ub] 135 | # since baseline does not support asymmetric action space 136 | scaled_action = (action+1)*0.5*(self.action_ub-self.action_lb)+self.action_lb 137 | scaled_action = np.clip(scaled_action, self.action_lb, self.action_ub) 138 | 139 | # feedback controller action 140 | action_fb = np.squeeze(self.controller.get_action(self.observation*self.observation_bound)) 141 | # total_action = policy_action + pid_action 142 | total_action = np.clip(scaled_action + action_fb, self.total_action_lb, self.total_action_ub) 143 | 144 | # check if reached target position/orientation 145 | flapper1_states = self.sim.states 146 | x = flapper1_states['body_positions'][3] 147 | x_dot = flapper1_states['body_spatial_velocities'][0] 148 | yaw_angle = flapper1_states['body_positions'][2] 149 | if self.reached == False and x > -0.05 and abs(x_dot) < 1 and abs(yaw_angle) < np.pi/4: 150 | self.reached = True 151 | if self.reached: 152 | total_action = np.clip((action_fb), self.total_action_lb, self.total_action_ub) 153 | 154 | # convert action to voltage signal 155 | max_voltage = total_action[0] 156 | voltage_diff = total_action[1] 157 | voltage_bias = total_action[2] 158 | split_cycle = total_action[3] 159 | input_voltage = np.zeros([2],dtype=np.float64) 160 | input_voltage[0] = self.generate_control_signal(self.sim.flapper1.frequency, max_voltage, voltage_diff, voltage_bias, -split_cycle, self.sim.world.time(), 0) 161 | input_voltage[1] = self.generate_control_signal(self.sim.flapper1.frequency, max_voltage, -voltage_diff, voltage_bias, split_cycle, self.sim.world.time(), 0) 162 | input_voltage[0] = np.clip(input_voltage[0], -18, 18) 163 | input_voltage[1] = np.clip(input_voltage[1], -18, 18) 164 | 165 | # run simulation and down sample from f_sim to f_control 166 | while self.sim.world.time() < self.next_control_time: 167 | self.sim.step(input_voltage) 168 | self.next_control_time += self.sim.dt_c 169 | 170 | # update control target 171 | self.mission.update_waypoint(self.sim.world.time()) 172 | 173 | # update observation, reward, terminal 174 | self.observation = self.get_observation() 175 | reward = self.get_reward(action) 176 | done = self.get_terminal() 177 | 178 | # visulization 179 | if self.is_visual_on and time.time() >= self.next_visualization_time: 180 | self.gui.cv.wait() 181 | self.next_visualization_time = time.time() + self.dt_v 182 | 183 | info = {'time' : self.sim.world.time()} 184 | return self.observation, reward, done, info 185 | 186 | def get_observation(self): 187 | # define observations here3 188 | # 189 | # observations are the following 190 | # rotation matrix 191 | # positions 192 | # linear velocities 193 | # angular velocities 194 | 195 | observation = np.zeros([18],dtype=np.float64) 196 | 197 | # get full states 198 | roll_angle = self.sim.sensor_fusion.out_roll_ 199 | pitch_angle = self.sim.sensor_fusion.out_pitch_ 200 | yaw_angle = self.sim.sensor_fusion.out_yaw_ 201 | 202 | R = self.euler_2_R(roll_angle, pitch_angle, yaw_angle) 203 | observation[0:9] = R.reshape(-1) 204 | 205 | observation[9] = self.sim.sensor_fusion.out_x_ - self.mission.pos_target_x_ 206 | observation[10] = self.sim.sensor_fusion.out_y_ - self.mission.pos_target_y_ 207 | observation[11] = self.sim.sensor_fusion.out_z_ - self.mission.pos_target_z_ 208 | observation[12] = self.sim.sensor_fusion.out_x_dot_ 209 | observation[13] = self.sim.sensor_fusion.out_y_dot_ 210 | observation[14] = self.sim.sensor_fusion.out_z_dot_ 211 | observation[15] = self.sim.sensor_fusion.IMU_gx_ 212 | observation[16] = self.sim.sensor_fusion.IMU_gy_ 213 | observation[17] = self.sim.sensor_fusion.IMU_gz_ 214 | 215 | observation = observation/self.observation_bound 216 | 217 | return observation 218 | 219 | def get_reward(self, action): 220 | reward = 0 221 | 222 | flapper1_states = self.sim.states 223 | 224 | position = flapper1_states['body_positions'][3:6] 225 | position_target = np.array([self.mission.pos_target_x_,self.mission.pos_target_y_,self.mission.pos_target_z_]) 226 | position_error = position_target - position 227 | 228 | angular_position = flapper1_states['body_positions'][0:3] 229 | angular_position_target = np.array([[0.0], [0.0], [0.0]]) 230 | angular_position_error = angular_position_target - angular_position 231 | 232 | linear_velocity = flapper1_states['body_spatial_velocities'] 233 | angular_velocity = flapper1_states['body_velocities'][0:3] 234 | 235 | d_action = action - self.action_old 236 | self.action_old = action 237 | 238 | control_cost = 2e-4*np.sum(np.square(action)) 239 | d_control_cost = 2*np.sum(np.square(d_action)) 240 | 241 | position_cost = 4e-3*np.linalg.norm(position_error) 242 | angular_position_cost = 8e-3*np.linalg.norm(angular_position_error) 243 | velocity_cots = 5e-4*np.linalg.norm(linear_velocity) 244 | angular_velocity_cost = 6e-4*np.linalg.norm(angular_velocity) 245 | 246 | stability_cost = position_cost + angular_position_cost + velocity_cots + angular_velocity_cost 247 | 248 | cost = (control_cost + d_control_cost + stability_cost)*3 249 | 250 | # body x axis projection along X axis 251 | R = self.sim.flapper1.flapper_skel.bodynode('torso').world_transform() 252 | i_hat = R[0:3,0] 253 | k_hat = R[0:3,2] 254 | x_projection = np.dot(i_hat,np.array([1,0,0])) 255 | z_projection = np.dot(k_hat,np.array([0,0,1])) 256 | 257 | # only negative reward when head (z) points downward 258 | if z_projection > 0: 259 | z_projection = 0 260 | 261 | #negative reward from x= -x_t to x=-0.05 262 | dis_yz_reward = flapper1_states['body_positions'][3]+0.05 263 | if dis_yz_reward > 0: 264 | dis_yz_reward = 0 265 | dis_yz_reward = 4* dis_yz_reward#scale to 1 266 | 267 | reward = 1/(cost**2+1e-6) + 4*x_projection + 1*z_projection + 6*dis_yz_reward 268 | 269 | # print("===========================time = %.4f ===========================" % self.world.time(), end="\n\r") 270 | # # # print("action", end="\n\r") 271 | # # # print(action, end="\n\r") 272 | # # # print("normalized_action", end="\n\r") 273 | # # # print(normalized_action, end="\n\r") 274 | # print("control_cost = %.8f" % control_cost, end="\n\r") 275 | # print("d_control_cost = %.8f" % d_control_cost, end="\n\r") 276 | # print("position_cost = %.8f" % position_cost, end="\n\r") 277 | # print("angular_position_cost = %.8f" % angular_position_cost, end="\n\r") 278 | # print("velocity_cots = %.8f" % velocity_cots, end="\n\r") 279 | # print("angular_velocity_cost = %.8f" % angular_velocity_cost, end="\n\r") 280 | # print("cost = %.8f" % cost, end="\n\r") 281 | # print("1/(cost**2+1e-6) = %.8f" % (1/(cost**2+1e-6)), end="\n\r") 282 | # print("4*_projection = %.8f" % (4*x_projection), end="\n\r") 283 | # print("1*z_projection = %.8f" % (1*z_projection), end="\n\r") 284 | # print("12*dis_yz_reward = %.8f" % (12*dis_yz_reward), end="\n\r") 285 | # print("reward = %.8f" % reward, end="\n\r") 286 | 287 | return reward 288 | 289 | def get_terminal(self): 290 | flapper1_states = self.sim.states 291 | x = flapper1_states['body_positions'][3] 292 | y = flapper1_states['body_positions'][4] 293 | z = flapper1_states['body_positions'][5] 294 | 295 | if self.sim.check_collision(): 296 | print("collided") 297 | return True 298 | if self.sim.world.time() > 3: 299 | print("overtime") 300 | return True 301 | if x > 0.1: 302 | print("x overshoot") 303 | return True 304 | if x < -0.3: 305 | print(self.observation[9]) 306 | print("x undershoot") 307 | return True 308 | if abs(y) > 0.2: 309 | print("y drifted") 310 | return True 311 | if abs(z) > 0.2: 312 | print("z drifted") 313 | return True 314 | R = self.sim.flapper1.flapper_skel.bodynode('torso').world_transform() 315 | i_hat = R[0:3,0] 316 | x_projection = np.dot(i_hat,np.array([1,0,0])) 317 | if self.sim.world.time() > 0.3 and x_projection<0: 318 | print("x aligned too slow") 319 | return True 320 | if self.sim.world.time() > 0.35 and x < -0.08: 321 | print("x translate too slow") 322 | return True 323 | return False 324 | 325 | def generate_control_signal(self, f, Umax, delta, bias, sc, t, phase_0): 326 | V = Umax + delta 327 | V0 = bias 328 | sigma = 0.5+sc 329 | 330 | T = 1/f 331 | t_phase = phase_0/360*T 332 | t = t+t_phase 333 | period = np.floor(t/T) 334 | t = t-period*T 335 | 336 | if 0<=t and t 3.49): 162 | if (self.out_yaw_ < self.out_yaw_old_): 163 | self.out_yaw_ = self.out_yaw_ + 6.28319 164 | else: 165 | self.out_yaw_ = self.out_yaw_ - 6.28319 166 | 167 | self.out_yaw_old_ = self.out_yaw_ 168 | 169 | def quaternion_to_euler_angle(self, w, x, y, z): 170 | 171 | t0 = +2.0 * (w * x + y * z) 172 | t1 = +1.0 - 2.0 * (x * x + y * y) 173 | X = math.atan2(t0, t1) 174 | 175 | t2 = +2.0 * (w * y - z * x) 176 | t2 = +1.0 if t2 > +1.0 else t2 177 | t2 = -1.0 if t2 < -1.0 else t2 178 | Y = math.asin(t2) 179 | 180 | t3 = +2.0 * (w * z + x * y) 181 | t4 = +1.0 - 2.0 * (y * y + z * z) 182 | Z = math.atan2(t3, t4) 183 | 184 | return X, Y, Z 185 | 186 | def euler_angle_to_quaternion(self, roll, pitch, yaw): 187 | w = np.cos(roll / 2.0) * np.cos(pitch / 2.0) * np.cos(yaw / 2.0) + np.sin(roll / 2.0) * np.sin(pitch / 2.0) * np.sin(yaw / 2.0) 188 | x = np.sin(roll / 2.0) * np.cos(pitch / 2.0) * np.cos(yaw / 2.0) - np.cos(roll / 2.0) * np.sin(pitch / 2.0) * np.sin(yaw / 2.0) 189 | y = np.cos(roll / 2.0) * np.sin(pitch / 2.0) * np.cos(yaw / 2.0) + np.sin(roll / 2.0) * np.cos(pitch / 2.0) * np.sin(yaw / 2.0) 190 | z = np.cos(roll / 2.0) * np.cos(pitch / 2.0) * np.sin(yaw / 2.0) - np.sin(roll / 2.0) * np.sin(pitch / 2.0) * np.cos(yaw / 2.0) 191 | 192 | return w, x, y, z 193 | 194 | def update_sensors(self, sensor): 195 | self.vicon_roll_ = sensor.vicon_roll_ 196 | self.vicon_pitch_ = sensor.vicon_pitch_ 197 | self.vicon_yaw_ = sensor.vicon_yaw_ 198 | self.vicon_x_ = sensor.vicon_x_ 199 | self.vicon_y_ = sensor.vicon_y_ 200 | self.vicon_z_ = sensor.vicon_z_ 201 | self.vicon_x_dot_ = sensor.vicon_x_dot_ 202 | self.vicon_y_dot_ = sensor.vicon_y_dot_ 203 | self.vicon_z_dot_ = sensor.vicon_z_dot_ 204 | 205 | self.IMU_ax_ = sensor.IMU_ax_ 206 | self.IMU_ay_ = sensor.IMU_ay_ 207 | self.IMU_az_ = sensor.IMU_az_ 208 | self.IMU_gx_ = sensor.IMU_gx_ 209 | self.IMU_gy_ = sensor.IMU_gy_ 210 | self.IMU_gz_ = sensor.IMU_gz_ 211 | self.IMU_mx_ = sensor.IMU_mx_ 212 | self.IMU_my_ = sensor.IMU_my_ 213 | self.IMU_mz_ = sensor.IMU_mz_ 214 | 215 | def EKF2_run(self, sensor, vicon_delay_step_): 216 | if sensor.vicon_update_flag_ == 0: #No vicon come in// only do priori estimation 217 | # update priori estimates using measurement instead of states 218 | self.gyro_[0][0] = self.IMU_gx_ 219 | self.gyro_[1][0] = self.IMU_gy_ 220 | self.gyro_[2][0] = self.IMU_gz_ 221 | self.accel_[0][0] = self.IMU_ax_ 222 | self.accel_[1][0] = self.IMU_ay_ 223 | self.accel_[2][0] = self.IMU_az_ 224 | self.x_po_[4][0] = self.gyro_[0][0] 225 | self.x_po_[5][0] = self.gyro_[1][0] 226 | self.x_po_[6][0] = self.gyro_[2][0] 227 | 228 | self.update_prediction() 229 | # update posterior covariance 230 | # put priori estimate in posterior estimate for output 231 | self.x_po_ = self.x_pri_ 232 | else: 233 | self.ind_prev = self.ind_now - vicon_delay_step_ 234 | if self.ind_prev < 0: 235 | j = self.ind_prev + self.EKF_cache_size_ 236 | else: 237 | j = self.ind_prev 238 | 239 | self.x_po_ = self.x_po_cache_[:, [j]] 240 | 241 | self.ind_prev_next = self.ind_prev + 1 242 | 243 | for j in range(self.ind_prev_next, self.ind_now + 2): 244 | if j < 0: 245 | i = j + self.EKF_cache_size_ 246 | else: 247 | i = j 248 | 249 | if j == self.ind_prev_next: 250 | self.gyro_ = self.gyro_cache_[:, [i]] 251 | self.accel_ = self.accel_cache_[:, [i]] 252 | roll_ref_ = self.vicon_roll_ 253 | pit_ref_ = self.vicon_pitch_ 254 | yaw_ref_ = self.vicon_yaw_ 255 | 256 | x_ref_ = self.vicon_x_ 257 | y_ref_ = self.vicon_y_ 258 | z_ref_ = self.vicon_z_ 259 | 260 | x_dot_ref_ = self.vicon_x_dot_ 261 | y_dot_ref_ = self.vicon_y_dot_ 262 | z_dot_ref_ = self.vicon_z_dot_ 263 | 264 | # s_phi_2 = np.sin(roll_ref_/2.0) 265 | # c_phi_2 = np.cos(roll_ref_/2.0) 266 | # s_theta_2 = np.sin(pit_ref_/2.0) 267 | # c_theta_2 = np.cos(pit_ref_/2.0) 268 | # s_psi_2 = np.sin(yaw_ref_/2.0) 269 | # c_psi_2 = np.cos(yaw_ref_/2.0) 270 | # self.x_po_[0][0] = c_phi_2*c_theta_2*c_psi_2 + s_phi_2*s_theta_2*s_psi_2 271 | # self.x_po_[1][0] = -c_phi_2*s_theta_2*s_psi_2 + c_theta_2*c_psi_2*s_phi_2 272 | # self.x_po_[2][0] = c_phi_2*c_psi_2*s_theta_2 + s_phi_2*c_theta_2*s_psi_2 273 | # self.x_po_[3][0] = c_phi_2*c_theta_2*s_psi_2 - s_phi_2*c_psi_2*s_theta_2 274 | 275 | # self.x_po_[0][0] = np.cos(roll_ref_/2.0)*np.cos(pit_ref_/2.0)*np.cos(yaw_ref_/2.0) + np.sin(roll_ref_/2.0)*np.sin(pit_ref_/2.0)*np.sin(yaw_ref_/2.0) 276 | # self.x_po_[1][0] = np.sin(roll_ref_/2.0)*np.cos(pit_ref_/2.0)*np.cos(yaw_ref_/2.0) - np.cos(roll_ref_/2.0)*np.sin(pit_ref_/2.0)*np.sin(yaw_ref_/2.0) 277 | # self.x_po_[2][0] = np.cos(roll_ref_/2.0)*np.sin(pit_ref_/2.0)*np.cos(yaw_ref_/2.0) + np.sin(roll_ref_/2.0)*np.cos(pit_ref_/2.0)*np.sin(yaw_ref_/2.0) 278 | # self.x_po_[3][0] = np.cos(roll_ref_/2.0)*np.cos(pit_ref_/2.0)*np.sin(yaw_ref_/2.0) - np.sin(roll_ref_/2.0)*np.sin(pit_ref_/2.0)*np.cos(yaw_ref_/2.0) 279 | [ 280 | self.x_po_[0][0], self.x_po_[1][0], self.x_po_[2][0], 281 | self.x_po_[3][0] 282 | ] = self.euler_angle_to_quaternion(roll_ref_, pit_ref_, 283 | yaw_ref_) 284 | self.x_po_[4][0] = self.gyro_[0][0] 285 | self.x_po_[5][0] = self.gyro_[1][0] 286 | self.x_po_[6][0] = self.gyro_[2][0] 287 | 288 | self.x_po_[7][0] = x_ref_ 289 | self.x_po_[8][0] = y_ref_ 290 | self.x_po_[9][0] = z_ref_ 291 | 292 | self.x_po_[10][0] = x_dot_ref_ 293 | self.x_po_[11][0] = y_dot_ref_ 294 | self.x_po_[12][0] = z_dot_ref_ 295 | 296 | self.update_prediction() 297 | self.x_po_ = self.x_pri_ 298 | else: 299 | if j == self.ind_now + 1: 300 | self.gyro_[0][0] = self.IMU_gx_ 301 | self.gyro_[1][0] = self.IMU_gy_ 302 | self.gyro_[2][0] = self.IMU_gz_ 303 | self.accel_[0][0] = self.IMU_ax_ 304 | self.accel_[1][0] = self.IMU_ay_ 305 | self.accel_[2][0] = self.IMU_az_ 306 | else: 307 | self.gyro_ = self.gyro_cache_[:, [i]] 308 | self.accel_ = self.accel_cache_[:, [i]] 309 | 310 | self.x_po_[4][0] = self.gyro_[0][0] 311 | self.x_po_[5][0] = self.gyro_[1][0] 312 | self.x_po_[6][0] = self.gyro_[2][0] 313 | 314 | self.update_prediction() 315 | self.x_po_ = self.x_pri_ 316 | 317 | if j != self.ind_now + 1: 318 | self.x_po_cache_[:, [i]] = self.x_po_ 319 | 320 | sensor.vicon_update_flag_ = 0 321 | 322 | self.ind_now = self.ind_now + 1 323 | if self.ind_now > self.EKF_cache_size_ - 1: 324 | self.ind_now = self.ind_now - self.EKF_cache_size_ 325 | 326 | self.x_po_cache_[:, [self.ind_now]] = self.x_po_ 327 | self.gyro_cache_[:, [self.ind_now]] = self.gyro_ 328 | self.accel_cache_[:, [self.ind_now]] = self.accel_ 329 | 330 | def update_prediction(self): 331 | ax = self.accel_[0][0] 332 | ay = self.accel_[1][0] 333 | az = self.accel_[2][0] 334 | q0 = self.x_po_[0][0] 335 | q1 = self.x_po_[1][0] 336 | q2 = self.x_po_[2][0] 337 | q3 = self.x_po_[3][0] 338 | 339 | R_ = np.zeros([3, 3], dtype=np.float64) 340 | 341 | R_[0][0] = q0**2 + q1**2 - q2**2 - q3**2 342 | R_[0][1] = 2 * (q1 * q2 + q0 * q3) 343 | R_[0][2] = 2 * (q1 * q3 - q0 * q2) 344 | R_[1][0] = 2 * (q1 * q2 - q0 * q3) 345 | R_[1][1] = q0**2 - q1**2 + q2**2 - q3**2 346 | R_[1][2] = 2 * (q2 * q3 + q0 * q1) 347 | R_[2][0] = 2 * (q1 * q3 + q0 * q2) 348 | R_[2][1] = 2 * (q2 * q3 - q0 * q1) 349 | R_[2][2] = q0**2 - q1**2 - q2**2 + q3**2 350 | 351 | self.x_pri_[0][0] = self.x_po_[0][0] - 0.5 * self.x_po_[1][0] * self.x_po_[4][0] * self.dt_s_ - 0.5 * self.x_po_[2][0] * self.x_po_[5][0] * self.dt_s_ - 0.5 * self.x_po_[3][0] * self.x_po_[6][0] * self.dt_s_ 352 | self.x_pri_[1][0] = self.x_po_[1][0] + 0.5 * self.x_po_[0][0] * self.x_po_[4][0] * self.dt_s_ - 0.5 * self.x_po_[3][0] * self.x_po_[5][0] * self.dt_s_ + 0.5 * self.x_po_[2][0] * self.x_po_[6][0] * self.dt_s_ 353 | self.x_pri_[2][0] = self.x_po_[2][0] + 0.5 * self.x_po_[3][0] * self.x_po_[4][0] * self.dt_s_ + 0.5 * self.x_po_[0][0] * self.x_po_[5][0] * self.dt_s_ - 0.5 * self.x_po_[1][0] * self.x_po_[6][0] * self.dt_s_ 354 | self.x_pri_[3][0] = self.x_po_[3][0] - 0.5 * self.x_po_[2][0] * self.x_po_[4][0] * self.dt_s_ + 0.5 * self.x_po_[1][0] * self.x_po_[5][0] * self.dt_s_ + 0.5 * self.x_po_[0][0] * self.x_po_[6][0] * self.dt_s_ 355 | # angular velocity 356 | self.x_pri_[4][0] = self.x_po_[4][0] + (self.I1_ * self.x_po_[5][0] * self.x_po_[6][0]) * self.dt_s_ 357 | self.x_pri_[5][0] = self.x_po_[5][0] + (self.I2_ * self.x_po_[4][0] * self.x_po_[6][0]) * self.dt_s_ 358 | self.x_pri_[6][0] = self.x_po_[6][0] + (self.I3_ * self.x_po_[4][0] * self.x_po_[5][0]) * self.dt_s_ 359 | # linear velocity 360 | self.p_ddot = np.matmul(np.transpose(R_), self.accel_) 361 | self.x_pri_[10][0] = self.x_po_[10][0] + self.p_ddot[0][0] * self.dt_s_ # + ((self.x_po_[0][0]**2 + self.x_po_[1][0]**2 - self.x_po_[2][0]**2 - self.x_po_[3][0]**2)*self.accel_[0][0] + 2*(self.x_po_[1][0]*self.x_po_[2][0] - self.x_po_[0][0]*self.x_po_[3][0])*self.accel_[1][0] + 2*(self.x_po_[0][0]*self.x_po_[2][0] + self.x_po_[1][0]*self.x_po_[3][0])*self.accel_[2][0])*self.dt_s_ 362 | self.x_pri_[11][0] = self.x_po_[11][0] + self.p_ddot[1][0] * self.dt_s_ # + (2*(self.x_po_[1][0]*self.x_po_[2][0] + self.x_po_[0][0]*self.x_po_[3][0])*self.accel_[0][0] + (self.x_po_[0][0]**2 - self.x_po_[1][0]**2 + self.x_po_[2][0]**2 - self.x_po_[3][0]**2)*self.accel_[1][0] + 2*(self.x_po_[2][0]*self.x_po_[3][0] - self.x_po_[0][0]*self.x_po_[1][0])*self.accel_[2][0])*self.dt_s_ 363 | self.x_pri_[12][0] = self.x_po_[12][0] + self.p_ddot[2][0] * self.dt_s_ # + (2*(self.x_po_[1][0]*self.x_po_[3][0] - self.x_po_[0][0]*self.x_po_[2][0])*self.accel_[0][0] + 2*(self.x_po_[0][0]*self.x_po_[1][0] + self.x_po_[2][0]*self.x_po_[3][0])*self.accel_[1][0] + (self.x_po_[0][0]**2 - self.x_po_[1][0]**2 - self.x_po_[2][0]**2 + self.x_po_[3][0]**2)*self.accel_[2][0])*self.dt_s_ 364 | # position 365 | self.x_pri_[7][0] = self.x_po_[7][0] + self.x_po_[10][0] * self.dt_s_ 366 | self.x_pri_[8][0] = self.x_po_[8][0] + self.x_po_[11][0] * self.dt_s_ 367 | self.x_pri_[9][0] = self.x_po_[9][0] + self.x_po_[12][0] * self.dt_s_ 368 | -------------------------------------------------------------------------------- /flappy/envs/fwmav/controllers/pid_controller.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class pid: 4 | def __init__(self): 5 | self.old_error = 0 6 | self.integral = 0 7 | self.int_max = 0 8 | self.Kp = 0 9 | self.Ki = 0 10 | self.Kd = 0 11 | self.p = 0 12 | self.i = 0 13 | self.d = 0 14 | 15 | class PIDController(): 16 | def __init__(self,dt): 17 | self.dt_ = dt 18 | self.desired_accel_x_ = 0 19 | self.desired_accel_y_ = 0 20 | 21 | self.pos_target_x_ = 0 22 | self.pos_target_y_ = 0 23 | self.pos_target_z_ = 0.0 24 | 25 | self.ang_ef_target_z_ = 0 26 | 27 | # control here 28 | # xy postion to velocity 29 | self.p_pos_xy_Kp_ = 7 #1m error to 0.1m/s 30 | 31 | # z postion to velocity 32 | self.p_pos_z_Kp_ = 5 33 | # z velocity to acceleration 34 | # self.p_vel_z_Kp_ = 20 # need to test 35 | 36 | # z position PID controller 37 | # self.pid_pos_z_ = pid() 38 | # self.pid_pos_z_.old_error = 0 39 | # self.pid_pos_z_.integral = 0 40 | # self.pid_pos_z_.int_max = 5 41 | # self.pid_pos_z_.Kp = 10 42 | # self.pid_pos_z_.Ki = 5 43 | # self.pid_pos_z_.Kd = 8 44 | # self.pid_pos_z_.p = 0 45 | # self.pid_pos_z_.i= 0 46 | # self.pid_pos_z_.d = 0 47 | 48 | # z velocity PID controller 49 | self.pid_vel_z_ = pid() 50 | self.pid_vel_z_.old_error = 0 51 | self.pid_vel_z_.integral = 0 52 | self.pid_vel_z_.int_max = 5 53 | self.pid_vel_z_.Kp = 10#10 54 | self.pid_vel_z_.Ki = 4#4 55 | self.pid_vel_z_.Kd = 0.04 56 | self.pid_vel_z_.p = 0 57 | self.pid_vel_z_.i= 0 58 | self.pid_vel_z_.d = 0 59 | 60 | # z acceleration PID controller 61 | # self.pid_acc_z_ = pid() 62 | # self.pid_acc_z_.old_error = 0 63 | # self.pid_acc_z_.integral = 0 64 | # self.pid_acc_z_.int_max = 5 65 | # self.pid_acc_z_.Kp = 1 66 | # self.pid_acc_z_.Ki = 0 67 | # self.pid_acc_z_.Kd = 0.00001 68 | # self.pid_acc_z_.p = 0 69 | # self.pid_acc_z_.i= 0 70 | # self.pid_acc_z_.d = 0 71 | 72 | # x velocity PID controller (pitch) 73 | self.pid_vel_x_ = pid() 74 | self.pid_vel_x_.old_error = 0 75 | self.pid_vel_x_.integral = 0 76 | self.pid_vel_x_.int_max = 10 77 | self.pid_vel_x_.Kp = 2 78 | self.pid_vel_x_.Ki = 0.5 79 | self.pid_vel_x_.Kd = 0.01 80 | self.pid_vel_x_.p = 0 81 | self.pid_vel_x_.i= 0 82 | self.pid_vel_x_.d = 0 83 | self.acc_target_xy_max_ = 10 84 | 85 | # y velocity PID controller (roll) 86 | self.pid_vel_y_ = pid() 87 | self.pid_vel_y_.old_error = 0 88 | self.pid_vel_y_.integral = 0 89 | self.pid_vel_y_.int_max = 10 90 | self.pid_vel_y_.Kp = 1.75 91 | self.pid_vel_y_.Ki = 0.4 92 | self.pid_vel_y_.Kd = 0.002 93 | self.pid_vel_y_.p = 0 94 | self.pid_vel_y_.i= 0 95 | self.pid_vel_y_.d = 0 96 | 97 | # rpy angle to angular rate // control how fast angle converges 98 | self.p_ang_roll_Kp_ = 10 99 | self.p_ang_pitch_Kp_ = 10 100 | self.p_ang_yaw_Kp_ = 5 101 | 102 | # roll angular rate PID controller 103 | self.pid_ang_roll_ = pid() 104 | self.pid_ang_roll_.old_error = 0 105 | self.pid_ang_roll_.integral = 0 106 | self.pid_ang_roll_.int_max = 2 107 | self.pid_ang_roll_.Kp = 3 108 | self.pid_ang_roll_.Ki = 1.5 109 | self.pid_ang_roll_.Kd = 0.3 110 | self.pid_ang_roll_.p = 0 111 | self.pid_ang_roll_.i= 0 112 | self.pid_ang_roll_.d = 0 113 | 114 | # pitch angular rate PID controller 115 | self.pid_ang_pitch_ = pid() 116 | self.pid_ang_pitch_.old_error = 0 117 | self.pid_ang_pitch_.integral = 0 118 | self.pid_ang_pitch_.int_max = 2 119 | self.pid_ang_pitch_.Kp = 5 120 | self.pid_ang_pitch_.Ki = 1.5 121 | self.pid_ang_pitch_.Kd = 0.4 122 | self.pid_ang_pitch_.p = 0 123 | self.pid_ang_pitch_.i= 0 124 | self.pid_ang_pitch_.d = 0 125 | 126 | # yaw angular rate PID controller 127 | self.pid_ang_yaw_ = pid() 128 | self.pid_ang_yaw_.old_error = 0 129 | self.pid_ang_yaw_.integral = 0 130 | self.pid_ang_yaw_.int_max = 1 131 | self.pid_ang_yaw_.Kp = 0.2 132 | self.pid_ang_yaw_.Ki = 0.33 133 | self.pid_ang_yaw_.Kd = 0.003 134 | self.pid_ang_yaw_.p = 0 135 | self.pid_ang_yaw_.i= 0 136 | self.pid_ang_yaw_.d = 0 137 | 138 | # control voltage limit 139 | self.differential_voltage_max_ = 3 140 | self.mean_voltage_max_ = 3.5 141 | self.split_cycle_max_ = 0.1 142 | self.hover_voltage_ = 9.3 143 | self.voltage_amplitude_max_ = 18 144 | 145 | self.frequency_ = 34 146 | self.voltage_amplitude_ = 12 147 | self.differential_voltage_ = 0 148 | self.mean_voltage_ = 0 149 | self.split_cycle_ = 0 150 | 151 | # working trim for flapper_sc_trim in mav_config_list.json 152 | # self.roll_trim_ = 0.4 #voltage 153 | # self.pitch_trim_ = -0.4 #voltage 154 | # self.yaw_trim_ = -0.04 #split cycle 155 | self.roll_trim_ = 0 #voltage 156 | self.pitch_trim_ = 0 #voltage 157 | self.yaw_trim_ = 0 #split cycle 158 | 159 | # filter 160 | RC = 1/(2*np.pi*20) 161 | self.alpha = dt/(RC+dt) 162 | RC = 1/(2*np.pi*20) 163 | self.alpha_xyz = dt/(RC+dt) 164 | 165 | self.pos_current_x_ = 0 166 | self.pos_current_y_ = 0 167 | self.vel_current_x_ = 0 168 | self.vel_current_y_ = 0 169 | self.roll_angle_ = 0 170 | self.pitch_angle_ = 0 171 | self.yaw_angle_ = 0 172 | self.gyro_x_ = 0 173 | self.gyro_y_ = 0 174 | self.gyro_z_ = 0 175 | self.altitude_ = 0 176 | self.velocity_z_ = 0 177 | self.acceleration_z_ = 0 178 | self.raw_velocity_z_old = 0 179 | 180 | 181 | def get_action(self, observation): 182 | self.sensor_read(observation) 183 | self.controller_run() 184 | self.add_trim() 185 | 186 | #print('differential_voltage_ = %.4f, mean_voltage_ = %.4f, split_cycle_ = %.4f' % (self.differential_voltage_, self.mean_voltage_, self.split_cycle_), end="\n\r") 187 | 188 | action_pid = np.zeros([4],dtype=np.float64) 189 | 190 | action_pid[0] = self.voltage_amplitude_ 191 | action_pid[1] = self.differential_voltage_ 192 | action_pid[2] = self.mean_voltage_ 193 | action_pid[3] = self.split_cycle_ 194 | 195 | return action_pid 196 | 197 | def controller_run(self): 198 | self.xy_control() 199 | self.attitude_control() 200 | self.z_control() 201 | 202 | def add_trim(self): 203 | self.differential_voltage_ = np.clip(self.differential_voltage_ + self.roll_trim_, -self.differential_voltage_max_, self.differential_voltage_max_) 204 | self.mean_voltage_ = np.clip(self.mean_voltage_ + self.pitch_trim_, -self.mean_voltage_max_, self.mean_voltage_max_) 205 | self.split_cycle_ = np.clip(self.split_cycle_ + self.yaw_trim_, -self.split_cycle_max_, self.split_cycle_max_) 206 | 207 | def sensor_read(self, observation): 208 | self.vel_current_x_old_ = self.vel_current_x_ 209 | self.vel_current_y_old_ = self.vel_current_y_ 210 | 211 | #updat raw observation 212 | raw_pos_current_x_ = observation[9] 213 | raw_pos_current_y_ = observation[10] 214 | raw_vel_current_x_ = observation[12] 215 | raw_vel_current_y_ = observation[13] 216 | 217 | R = observation[0:9] 218 | [raw_roll_angle_, raw_pitch_angle_, raw_yaw_angle_] = self.rotation_to_euler_angle(R.reshape(3,3)) 219 | 220 | raw_gyro_x_ = observation[15] 221 | raw_gyro_y_ = observation[16] 222 | raw_gyro_z_ = observation[17] 223 | raw_altitude_ = observation[11] 224 | raw_velocity_z_ = observation[14] 225 | raw_acceleration_z_ = (raw_velocity_z_ - self.raw_velocity_z_old)/self.dt_ 226 | self.raw_velocity_z_old = raw_velocity_z_ 227 | 228 | # filter with low pass 229 | self.pos_current_x_ = self.pos_current_x_*(1-self.alpha_xyz) + raw_pos_current_x_*self.alpha_xyz 230 | self.pos_current_y_ = self.pos_current_y_*(1-self.alpha_xyz) + raw_pos_current_y_*self.alpha_xyz 231 | self.vel_current_x_ = self.vel_current_x_*(1-self.alpha_xyz) + raw_vel_current_x_*self.alpha_xyz 232 | self.vel_current_y_ = self.vel_current_y_*(1-self.alpha_xyz) + raw_vel_current_y_*self.alpha_xyz 233 | self.roll_angle_ = self.roll_angle_*(1-self.alpha) + raw_roll_angle_*self.alpha 234 | self.pitch_angle_ = self.pitch_angle_*(1-self.alpha) + raw_pitch_angle_*self.alpha 235 | self.yaw_angle_ = self.yaw_angle_*(1-self.alpha) + raw_yaw_angle_*self.alpha 236 | self.gyro_x_ = self.gyro_x_*(1-self.alpha) + raw_gyro_x_*self.alpha 237 | self.gyro_y_ = self.gyro_y_*(1-self.alpha) + raw_gyro_y_*self.alpha 238 | self.gyro_z_ = self.gyro_z_*(1-self.alpha) + raw_gyro_z_*self.alpha 239 | self.altitude_ = self.altitude_*(1-self.alpha_xyz) + raw_altitude_*self.alpha_xyz 240 | self.velocity_z_ = self.velocity_z_*(1-self.alpha_xyz) + raw_velocity_z_*self.alpha_xyz 241 | self.acceleration_z_ = self.acceleration_z_*(1-self.alpha_xyz) + raw_acceleration_z_*self.alpha_xyz 242 | 243 | self.sin_roll_ = np.sin(self.roll_angle_) 244 | self.cos_roll_ = np.cos(self.roll_angle_) 245 | self.sin_pitch_ = np.sin(self.pitch_angle_) 246 | self.cos_pitch_ = np.cos(self.pitch_angle_) 247 | self.sin_yaw_ = np.sin(self.yaw_angle_) 248 | self.cos_yaw_ = np.cos(self.yaw_angle_) 249 | 250 | # derivatives 251 | self.acc_current_x_ = (self.vel_current_x_ - self.vel_current_x_old_)/self.dt_ 252 | self.acc_current_y_ = (self.vel_current_y_ - self.vel_current_y_old_)/self.dt_ 253 | 254 | def xy_control(self): 255 | # desired acceleration to velocity 256 | desired_vel_x = self.desired_accel_x_ * self.dt_ 257 | desired_vel_y = self.desired_accel_y_ * self.dt_ 258 | 259 | # update xy controller 260 | # desired velocity to position 261 | self.pos_target_x_ = self.pos_target_x_ + desired_vel_x * self.dt_ 262 | self.pos_target_y_ = self.pos_target_y_ + desired_vel_y * self.dt_ 263 | 264 | # position to rate 265 | pos_error_x = self.pos_target_x_ - self.pos_current_x_ 266 | pos_error_y = self.pos_target_y_ - self.pos_current_y_ 267 | vel_target_x = self.p_pos_xy_Kp_ * pos_error_x 268 | vel_target_y = self.p_pos_xy_Kp_ * pos_error_y 269 | 270 | # rate to acceleration 271 | vel_error_x = vel_target_x - self.vel_current_x_ 272 | vel_error_y = vel_target_y - self.vel_current_y_ 273 | 274 | self.pid_vel_x_ = self.update_pid(vel_error_x, self.pid_vel_x_) 275 | acc_target_x = np.clip(self.pid_vel_x_.p+self.pid_vel_x_.i+self.pid_vel_x_.d, -self.acc_target_xy_max_, self.acc_target_xy_max_) 276 | #print('x_p = %.4f, x_i = %.4f, x_d = %.4f' % (self.pid_vel_x_.p, self.pid_vel_x_.i, self.pid_vel_x_.d), end="\n\r") 277 | self.pid_vel_y_ = self.update_pid(vel_error_y, self.pid_vel_y_) 278 | acc_target_y = np.clip(self.pid_vel_y_.p+self.pid_vel_y_.i+self.pid_vel_y_.d, -self.acc_target_xy_max_, self.acc_target_xy_max_) 279 | #print('y_p = %.4f, y_i = %.4f, y_d = %.4f' % (self.pid_vel_y_.p, self.pid_vel_y_.i, self.pid_vel_y_.d), end="\n\r") 280 | 281 | # acceleration to lean angle 282 | acc_fwd = acc_target_x * self.cos_yaw_ + acc_target_y * self.sin_yaw_ 283 | acc_lft = -acc_target_x * self.sin_yaw_ + acc_target_y * self.cos_yaw_ 284 | 285 | self.pitch_target_ef_ = np.arctan(acc_fwd/9.81) 286 | self.roll_target_ef_ = -np.arctan(acc_lft*np.cos(self.pitch_target_ef_)/9.81) 287 | #print('roll target = %.4f, pitch target = %.4f' % (self.roll_target_ef_/np.pi*180, self.pitch_target_ef_/np.pi*180), end="\n\r") 288 | 289 | def attitude_control(self): 290 | # constraint roll pitch angle target 291 | # for debug 292 | #roll_target_ef_ = 0; 293 | #pitch_target_ef_ = 0; 294 | ang_ef_target_x = np.clip(self.roll_target_ef_, -45.0/180.0*np.pi, 45.0/180.0*np.pi) #roll can be 45 deg 295 | ang_ef_target_y = np.clip(self.pitch_target_ef_, -30.0/180.0*np.pi, 30.0/180.0*np.pi) 296 | 297 | ang_ef_error_x = self.wrap_180(ang_ef_target_x - self.roll_angle_) 298 | ang_ef_error_y = self.wrap_180(ang_ef_target_y - self.pitch_angle_) 299 | ang_ef_error_z = self.wrap_180(self.ang_ef_target_z_ - self.yaw_angle_) 300 | ang_ef_error_z = np.clip(ang_ef_error_z, -10/180*np.pi, 10/180*np.pi) 301 | 302 | # convert to body frame 303 | 304 | ang_bf_error = self.frame_ef_to_bf(ang_ef_error_x, ang_ef_error_y, ang_ef_error_z) 305 | ang_bf_error_x = ang_bf_error[0] 306 | ang_bf_error_y = ang_bf_error[1] 307 | ang_bf_error_z = ang_bf_error[2] 308 | # ang_bf_error_z = 0.0f; 309 | #print('ang_ef_target_z_ = %.4f, yaw_angle_ = %.4f, ang_bf_error_z = %.4f' % (self.ang_ef_target_z_, self.yaw_angle_, ang_bf_error_z), end="\n\r") 310 | 311 | # roll angle to roll control 312 | self.pid_ang_roll_ = self.update_pid(ang_bf_error_x, self.pid_ang_roll_) 313 | self.differential_voltage_ = np.clip(self.pid_ang_roll_.p + self.pid_ang_roll_.i + self.pid_ang_roll_.d, -self.differential_voltage_max_, self.differential_voltage_max_) 314 | #print('roll_p = %.4f, roll_i = %.4f, roll_d = %.4f' % (self.pid_ang_roll_.p, self.pid_ang_roll_.i, self.pid_ang_roll_.d), end="\n\r") 315 | 316 | self.pid_ang_pitch_ = self.update_pid(ang_bf_error_y, self.pid_ang_pitch_) 317 | self.mean_voltage_ = np.clip(self.pid_ang_pitch_.p + self.pid_ang_pitch_.i + self.pid_ang_pitch_.d, -self.mean_voltage_max_, self.mean_voltage_max_) 318 | #print('pitch_p = %.4f, pitch_i = %.4f, pitch_d = %.4f' % (self.pid_ang_pitch_.p, self.pid_ang_pitch_.i, self.pid_ang_pitch_.d), end="\n\r") 319 | 320 | self.pid_ang_yaw_ = self.update_pid(ang_bf_error_z, self.pid_ang_yaw_) 321 | self.split_cycle_ = np.clip(self.pid_ang_yaw_.p + self.pid_ang_yaw_.i + self.pid_ang_yaw_.d, -self.split_cycle_max_, self.split_cycle_max_) 322 | #print('yaw_p = %.4f, yaw_i = %.4f, yaw_d = %.4f' % (self.pid_ang_yaw_.p, self.pid_ang_yaw_.i, self.pid_ang_yaw_.d), end="\n\r") 323 | 324 | # update rate body frame targets 325 | # rate_bf_target_x = self.p_ang_roll_Kp_ * ang_bf_error_x 326 | # rate_bf_target_y = self.p_ang_pitch_Kp_ * ang_bf_error_y 327 | # rate_bf_target_z = self.p_ang_yaw_Kp_ * ang_bf_error_z 328 | 329 | #include roll and pitch rate required to account for precession of the desired attitude about the body frame yaw axes 330 | #rate_bf_target_x = rate_bf_target_x + ang_bf_error_y* gyro_z_; 331 | #rate_bf_target_y = rate_bf_target_y - ang_bf_error_x* gyro_z_; 332 | 333 | # run rate controller 334 | # self.rate_bf_to_roll(rate_bf_target_x) 335 | # self.rate_bf_to_pitch(rate_bf_target_y) 336 | # self.rate_bf_to_yaw(rate_bf_target_z) 337 | 338 | def wrap_180(self, angle): 339 | if (angle > 3*np.pi or angle < -3*np.pi): 340 | angle = np.fmod(angle,2*np.pi) 341 | if (angle > np.pi): 342 | angle = angle - 2*np.pi 343 | if (angle < - np.pi): 344 | angle = angle + 2*np.pi 345 | return angle; 346 | 347 | def rate_bf_to_roll(self, rate_target): 348 | rate_error = rate_target - self.gyro_x_ 349 | self.pid_rate_roll_ = self.update_pid(rate_error, self.pid_rate_roll_) 350 | self.differential_voltage_ = np.clip(self.pid_rate_roll_.p + self.pid_rate_roll_.i + self.pid_rate_roll_.d, -self.differential_voltage_max_, self.differential_voltage_max_) 351 | 352 | def rate_bf_to_pitch(self, rate_target): 353 | rate_error = rate_target - self.gyro_y_ 354 | self.pid_rate_pitch_ = self.update_pid(rate_error, self.pid_rate_pitch_) 355 | self.mean_voltage_ = np.clip(self.pid_rate_pitch_.p + self.pid_rate_pitch_.i + self.pid_rate_pitch_.d, -self.mean_voltage_max_, self.mean_voltage_max_) 356 | 357 | def rate_bf_to_yaw(self, rate_target): 358 | rate_error = rate_target - self.gyro_z_ 359 | self.pid_rate_yaw_ = self.update_pid(rate_error, self.pid_rate_yaw_) 360 | self.split_cycle_ = np.clip(self.pid_rate_yaw_.p + self.pid_rate_yaw_.i + self.pid_rate_yaw_.d, -self.split_cycle_max_, self.split_cycle_max_) 361 | 362 | def update_pid(self, error, pid_target, gyro = 0): 363 | integral = pid_target.integral + error*self.dt_ 364 | 365 | if (integral > pid_target.int_max): 366 | integral = pid_target.int_max 367 | elif (integral < -pid_target.int_max): 368 | integral = -pid_target.int_max 369 | 370 | if gyro == 0: 371 | derivative = (error - pid_target.old_error)/self.dt_ 372 | else: 373 | derivative = gyro 374 | pid_target.p = error*pid_target.Kp 375 | pid_target.i = integral*pid_target.Ki 376 | pid_target.d = derivative*pid_target.Kd 377 | 378 | # update all fields 379 | pid_target.old_error = error 380 | pid_target.integral = integral 381 | 382 | return pid_target 383 | 384 | def z_control(self): 385 | # position to rate 386 | pos_error_z = self.pos_target_z_ - self.altitude_ 387 | #pos_error_z = np.clip(pos_error_z, -0.2, 0.2) 388 | # self.pid_pos_z_ = self.update_pid(pos_error_z, self.pid_pos_z_) 389 | # voltage_out = self.pid_pos_z_.p + self.pid_pos_z_.i + self.pid_pos_z_.d 390 | # print('z_p = %.4f, z_i = %.4f, z_d = %.4f' % (self.pid_pos_z_.p, self.pid_pos_z_.i, self.pid_pos_z_.d), end="\n\r") 391 | vel_target_z = pos_error_z * self.p_pos_z_Kp_ 392 | 393 | # rate to acceleration 394 | vel_error_z = vel_target_z - self.velocity_z_ 395 | 396 | self.pid_vel_z_ = self.update_pid(vel_error_z, self.pid_vel_z_) 397 | voltage_out = self.pid_vel_z_.p + self.pid_vel_z_.i + self.pid_vel_z_.d 398 | #print('z_p = %.4f, z_i = %.4f, z_d = %.4f' % (self.pid_vel_z_.p, self.pid_vel_z_.i, self.pid_vel_z_.d), end="\n\r") 399 | # acc_target_z = vel_error_z * self.p_vel_z_Kp_ 400 | 401 | # # acceleration to throttle 402 | # acc_error_z = acc_target_z - self.acceleration_z_ 403 | 404 | # self.pid_acc_z_ = self.update_pid(acc_error_z, self.pid_acc_z_) 405 | 406 | # voltage_out = self.pid_acc_z_.p + self.pid_acc_z_.i + self.pid_acc_z_.d 407 | voltage_out = voltage_out/(self.cos_pitch_*self.cos_roll_) 408 | 409 | self.voltage_amplitude_ = np.clip(voltage_out, 0, self.voltage_amplitude_max_ - self.hover_voltage_) 410 | self.voltage_amplitude_ += self.hover_voltage_ 411 | #print('voltage_amplitude_ = %.4f' % self.voltage_amplitude_, end="\n\r") 412 | 413 | def frame_ef_to_bf(self, ef_x, ef_y, ef_z): 414 | bf = np.zeros([3],dtype=np.float64) 415 | 416 | bf[0] = ef_x - self.sin_pitch_*ef_z 417 | bf[1] = self.cos_roll_*ef_y + self.sin_roll_*self.cos_pitch_*ef_z 418 | bf[2] = -self.sin_roll_*ef_y + self.cos_pitch_*self.cos_roll_*ef_z 419 | 420 | return bf 421 | 422 | def rotation_to_euler_angle(self,R): 423 | roll = np.arctan2(R[2,1],R[2,2]) 424 | pitch = np.arcsin(-R[2, 0]) 425 | yaw = np.arctan2(R[1,0],R[0,0]) 426 | return roll, pitch, yaw 427 | 428 | -------------------------------------------------------------------------------- /flappy/envs/fwmav/controllers/controller_no_base.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class pid: 4 | def __init__(self): 5 | self.old_error = 0 6 | self.integral = 0 7 | self.int_max = 0 8 | self.Kp = 0 9 | self.Ki = 0 10 | self.Kd = 0 11 | self.p = 0 12 | self.i = 0 13 | self.d = 0 14 | 15 | class PIDController(): 16 | def __init__(self,dt): 17 | 18 | self.desired_accel_x_ = 0 19 | self.desired_accel_y_ = 0 20 | 21 | self.pos_target_x_ = 0 22 | self.pos_target_y_ = 0 23 | self.pos_target_z_ = 0.5 24 | 25 | self.ang_ef_target_z_ = 0 26 | 27 | # control here 28 | # xy postion to velocity 29 | self.p_pos_xy_Kp_ = 7 #1m error to 0.1m/s 30 | 31 | # z postion to velocity 32 | self.p_pos_z_Kp_ = 1 33 | # z velocity to acceleration 34 | # self.p_vel_z_Kp_ = 20 # need to test 35 | 36 | # z position PID controller 37 | # self.pid_pos_z_ = pid() 38 | # self.pid_pos_z_.old_error = 0 39 | # self.pid_pos_z_.integral = 0 40 | # self.pid_pos_z_.int_max = 5 41 | # self.pid_pos_z_.Kp = 10 42 | # self.pid_pos_z_.Ki = 5 43 | # self.pid_pos_z_.Kd = 8 44 | # self.pid_pos_z_.p = 0 45 | # self.pid_pos_z_.i= 0 46 | # self.pid_pos_z_.d = 0 47 | 48 | # z velocity PID controller 49 | self.pid_vel_z_ = pid() 50 | self.pid_vel_z_.old_error = 0 51 | self.pid_vel_z_.integral = 0 52 | self.pid_vel_z_.int_max = 5 53 | self.pid_vel_z_.Kp = 10 54 | self.pid_vel_z_.Ki = 4 55 | self.pid_vel_z_.Kd = 0.04 56 | self.pid_vel_z_.p = 0 57 | self.pid_vel_z_.i= 0 58 | self.pid_vel_z_.d = 0 59 | 60 | # z acceleration PID controller 61 | # self.pid_acc_z_ = pid() 62 | # self.pid_acc_z_.old_error = 0 63 | # self.pid_acc_z_.integral = 0 64 | # self.pid_acc_z_.int_max = 5 65 | # self.pid_acc_z_.Kp = 1 66 | # self.pid_acc_z_.Ki = 0 67 | # self.pid_acc_z_.Kd = 0.00001 68 | # self.pid_acc_z_.p = 0 69 | # self.pid_acc_z_.i= 0 70 | # self.pid_acc_z_.d = 0 71 | 72 | # x velocity PID controller (pitch) 73 | self.pid_vel_x_ = pid() 74 | self.pid_vel_x_.old_error = 0 75 | self.pid_vel_x_.integral = 0 76 | self.pid_vel_x_.int_max = 10 77 | self.pid_vel_x_.Kp = 2 78 | self.pid_vel_x_.Ki = 0.5 79 | self.pid_vel_x_.Kd = 0.01 80 | self.pid_vel_x_.p = 0 81 | self.pid_vel_x_.i= 0 82 | self.pid_vel_x_.d = 0 83 | self.acc_target_xy_max_ = 10 84 | 85 | # y velocity PID controller (roll) 86 | self.pid_vel_y_ = pid() 87 | self.pid_vel_y_.old_error = 0 88 | self.pid_vel_y_.integral = 0 89 | self.pid_vel_y_.int_max = 10 90 | self.pid_vel_y_.Kp = 1.75 91 | self.pid_vel_y_.Ki = 0.4 92 | self.pid_vel_y_.Kd = 0.002 93 | self.pid_vel_y_.p = 0 94 | self.pid_vel_y_.i= 0 95 | self.pid_vel_y_.d = 0 96 | 97 | # rpy angle to angular rate // control how fast angle converges 98 | self.p_ang_roll_Kp_ = 10 99 | self.p_ang_pitch_Kp_ = 10 100 | self.p_ang_yaw_Kp_ = 5 101 | 102 | # roll angular rate PID controller 103 | self.pid_ang_roll_ = pid() 104 | self.pid_ang_roll_.old_error = 0 105 | self.pid_ang_roll_.integral = 0 106 | self.pid_ang_roll_.int_max = 2 107 | self.pid_ang_roll_.Kp = 3 108 | self.pid_ang_roll_.Ki = 1.5 109 | self.pid_ang_roll_.Kd = 0.4 110 | self.pid_ang_roll_.p = 0 111 | self.pid_ang_roll_.i= 0 112 | self.pid_ang_roll_.d = 0 113 | 114 | # pitch angular rate PID controller 115 | self.pid_ang_pitch_ = pid() 116 | self.pid_ang_pitch_.old_error = 0 117 | self.pid_ang_pitch_.integral = 0 118 | self.pid_ang_pitch_.int_max = 2 119 | self.pid_ang_pitch_.Kp = 5 120 | self.pid_ang_pitch_.Ki = 1.5 121 | self.pid_ang_pitch_.Kd = 0.4 122 | self.pid_ang_pitch_.p = 0 123 | self.pid_ang_pitch_.i= 0 124 | self.pid_ang_pitch_.d = 0 125 | 126 | # yaw angular rate PID controller 127 | self.pid_ang_yaw_ = pid() 128 | self.pid_ang_yaw_.old_error = 0 129 | self.pid_ang_yaw_.integral = 0 130 | self.pid_ang_yaw_.int_max = 0.1 131 | self.pid_ang_yaw_.Kp = 0.2 132 | self.pid_ang_yaw_.Ki = 0.1 133 | self.pid_ang_yaw_.Kd = 0.0001 134 | self.pid_ang_yaw_.p = 0 135 | self.pid_ang_yaw_.i= 0 136 | self.pid_ang_yaw_.d = 0 137 | 138 | # control voltage limit 139 | self.differential_voltage_max_ = 3.5 140 | self.mean_voltage_max_ = 2.5 141 | self.split_cycle_max_ = 0.15 142 | self.hover_voltage_ = 9.0 143 | self.voltage_amplitude_max_ = 18 144 | 145 | self.frequency_ = 34 146 | self.voltage_amplitude_ = 12 147 | self.differential_voltage_ = 0 148 | self.mean_voltage_ = 0 149 | self.split_cycle_ = 0 150 | 151 | # working trim 152 | self.roll_trim_ = 0.0; #voltage 153 | self.pitch_trim_ = -0.4; #voltage 154 | self.yaw_trim_ = -0.04; #split cycle 155 | 156 | # filter 157 | RC = 1/(2*np.pi*20) 158 | self.alpha = dt/(RC+dt) 159 | RC = 1/(2*np.pi*20) 160 | self.alpha_xyz = dt/(RC+dt) 161 | 162 | self.pos_current_x_ = 0 163 | self.pos_current_y_ = 0 164 | self.vel_current_x_ = 0 165 | self.vel_current_y_ = 0 166 | self.roll_angle_ = 0 167 | self.pitch_angle_ = 0 168 | self.yaw_angle_ = 0 169 | self.gyro_x_ = 0 170 | self.gyro_y_ = 0 171 | self.gyro_z_ = 0 172 | self.altitude_ = 0 173 | self.velocity_z_ = 0 174 | self.acceleration_z_ = 0 175 | 176 | 177 | def get_action(self, states, dt): 178 | self.dt_ = dt 179 | self.sensor_read(states) 180 | self.controller_run() 181 | self.add_trim() 182 | 183 | print('differential_voltage_ = %.4f, mean_voltage_ = %.4f, split_cycle_ = %.4f, voltage_amplitude_ = %.4f' % (self.differential_voltage_, self.mean_voltage_, self.split_cycle_, self.voltage_amplitude_), end="\n\r") 184 | 185 | action_pid = np.zeros([4],dtype=np.float64) 186 | 187 | action_pid[0] = self.voltage_amplitude_ 188 | action_pid[1] = self.differential_voltage_ 189 | action_pid[2] = self.mean_voltage_ 190 | action_pid[3] = self.split_cycle_ 191 | 192 | return action_pid 193 | 194 | def controller_run(self): 195 | self.xy_control() 196 | self.attitude_control() 197 | self.z_control() 198 | 199 | def add_trim(self): 200 | self.differential_voltage_ = np.clip(self.differential_voltage_ + self.roll_trim_, -self.differential_voltage_max_, self.differential_voltage_max_) 201 | self.mean_voltage_ = np.clip(self.mean_voltage_ + self.pitch_trim_, -self.mean_voltage_max_, self.mean_voltage_max_) 202 | self.split_cycle_ = np.clip(self.split_cycle_ + self.yaw_trim_, -self.split_cycle_max_, self.split_cycle_max_) 203 | 204 | def sensor_read(self,states): 205 | raw_pos_current_x_ = states['body_positions'][3,0] + np.random.normal(0,0.0005) 206 | raw_pos_current_y_ = states['body_positions'][4,0] + np.random.normal(0,0.0005) 207 | raw_vel_current_x_ = states['body_spatial_velocities'][0,0] + np.random.normal(0,0.0005) 208 | raw_vel_current_y_ = states['body_spatial_velocities'][1,0] + np.random.normal(0,0.0005) 209 | raw_roll_angle_ = states['body_positions'][0,0] + np.random.normal(0,2/180*np.pi) 210 | raw_pitch_angle_ = states['body_positions'][1,0] + np.random.normal(0,2/180*np.pi) 211 | raw_yaw_angle_ = states['body_positions'][2,0] + np.random.normal(0,2/180*np.pi) 212 | raw_gyro_x_ = states['body_velocities'][0,0] + np.random.normal(0.01,5/180*np.pi) 213 | raw_gyro_y_ = states['body_velocities'][1,0] + np.random.normal(0.01,5/180*np.pi) 214 | raw_gyro_z_ = states['body_velocities'][2,0] + np.random.normal(0.01,5/180*np.pi) 215 | raw_altitude_ = states['body_positions'][5,0] + np.random.normal(0,0.0005) 216 | raw_velocity_z_ = states['body_spatial_velocities'][2,0] + np.random.normal(0,0.005) 217 | raw_acceleration_z_ = states['body_spatial_accelerations'][2,0] + np.random.normal(0,0.05) 218 | 219 | raw_pos_current_x_ = states['body_positions'][3,0] 220 | raw_pos_current_y_ = states['body_positions'][4,0] 221 | raw_vel_current_x_ = states['body_spatial_velocities'][0,0] 222 | raw_vel_current_y_ = states['body_spatial_velocities'][1,0] 223 | raw_roll_angle_ = states['body_positions'][0,0] 224 | raw_pitch_angle_ = states['body_positions'][1,0] 225 | raw_yaw_angle_ = states['body_positions'][2,0] 226 | raw_gyro_x_ = states['body_velocities'][0,0] 227 | raw_gyro_y_ = states['body_velocities'][1,0] 228 | raw_gyro_z_ = states['body_velocities'][2,0] 229 | raw_altitude_ = states['body_positions'][5,0] 230 | raw_velocity_z_ = states['body_spatial_velocities'][2,0] 231 | raw_acceleration_z_ = states['body_spatial_accelerations'][2,0] 232 | 233 | 234 | # filter z with 34 Hz low pass 235 | self.pos_current_x_ = self.pos_current_x_*(1-self.alpha_xyz) + raw_pos_current_x_*self.alpha_xyz 236 | self.pos_current_y_ = self.pos_current_y_*(1-self.alpha_xyz) + raw_pos_current_y_*self.alpha_xyz 237 | self.vel_current_x_ = self.vel_current_x_*(1-self.alpha_xyz) + raw_vel_current_x_*self.alpha_xyz 238 | self.vel_current_y_ = self.vel_current_y_*(1-self.alpha_xyz) + raw_vel_current_y_*self.alpha_xyz 239 | self.roll_angle_ = self.roll_angle_*(1-self.alpha) + raw_roll_angle_*self.alpha 240 | self.pitch_angle_ = self.pitch_angle_*(1-self.alpha) + raw_pitch_angle_*self.alpha 241 | self.yaw_angle_ = self.yaw_angle_*(1-self.alpha) + raw_yaw_angle_*self.alpha 242 | self.gyro_x_ = self.gyro_x_*(1-self.alpha) + raw_gyro_x_*self.alpha 243 | self.gyro_y_ = self.gyro_y_*(1-self.alpha) + raw_gyro_y_*self.alpha 244 | self.gyro_z_ = self.gyro_z_*(1-self.alpha) + raw_gyro_z_*self.alpha 245 | self.altitude_ = self.altitude_*(1-self.alpha_xyz) + raw_altitude_*self.alpha_xyz 246 | self.velocity_z_ = self.velocity_z_*(1-self.alpha_xyz) + raw_velocity_z_*self.alpha_xyz 247 | self.acceleration_z_ = self.acceleration_z_*(1-self.alpha_xyz) + raw_acceleration_z_*self.alpha_xyz 248 | 249 | # self.pos_current_x_ = raw_pos_current_x_ 250 | # self.pos_current_y_ = raw_pos_current_y_ 251 | # self.vel_current_x_ = raw_vel_current_x_ 252 | # self.vel_current_y_ = raw_vel_current_y_ 253 | # self.roll_angle_ = raw_roll_angle_ 254 | # self.pitch_angle_ = raw_pitch_angle_ 255 | # self.yaw_angle_ = raw_yaw_angle_ 256 | # self.gyro_x_ = raw_gyro_x_ 257 | # self.gyro_y_ = raw_gyro_y_ 258 | # self.gyro_z_ = raw_gyro_z_ 259 | 260 | # self.altitude_ = raw_altitude_ 261 | # self.velocity_z_ = raw_velocity_z_ 262 | # self.acceleration_z_ = raw_acceleration_z_ 263 | 264 | self.sin_roll_ = np.sin(self.roll_angle_) 265 | self.cos_roll_ = np.cos(self.roll_angle_) 266 | self.sin_pitch_ = np.sin(self.pitch_angle_) 267 | self.cos_pitch_ = np.cos(self.pitch_angle_) 268 | self.sin_yaw_ = np.sin(self.yaw_angle_) 269 | self.cos_yaw_ = np.cos(self.yaw_angle_) 270 | 271 | 272 | 273 | def xy_control(self): 274 | # desired acceleration to velocity 275 | desired_vel_x = self.desired_accel_x_ * self.dt_ 276 | desired_vel_y = self.desired_accel_y_ * self.dt_ 277 | 278 | # update xy controller 279 | # desired velocity to position 280 | self.pos_target_x_ = self.pos_target_x_ + desired_vel_x * self.dt_ 281 | self.pos_target_y_ = self.pos_target_y_ + desired_vel_y * self.dt_ 282 | 283 | # position to rate 284 | pos_error_x = self.pos_target_x_ - self.pos_current_x_ 285 | pos_error_y = self.pos_target_y_ - self.pos_current_y_ 286 | vel_target_x = self.p_pos_xy_Kp_ * pos_error_x 287 | vel_target_y = self.p_pos_xy_Kp_ * pos_error_y 288 | 289 | # rate to acceleration 290 | vel_error_x = vel_target_x - self.vel_current_x_ 291 | vel_error_y = vel_target_y - self.vel_current_y_ 292 | 293 | self.pid_vel_x_ = self.update_pid(vel_error_x, self.pid_vel_x_) 294 | acc_target_x = np.clip(self.pid_vel_x_.p+self.pid_vel_x_.i+self.pid_vel_x_.d, -self.acc_target_xy_max_, self.acc_target_xy_max_) 295 | print('x_p = %.4f, x_i = %.4f, x_d = %.4f' % (self.pid_vel_x_.p, self.pid_vel_x_.i, self.pid_vel_x_.d), end="\n\r") 296 | self.pid_vel_y_ = self.update_pid(vel_error_y, self.pid_vel_y_) 297 | acc_target_y = np.clip(self.pid_vel_y_.p+self.pid_vel_y_.i+self.pid_vel_y_.d, -self.acc_target_xy_max_, self.acc_target_xy_max_) 298 | print('y_p = %.4f, y_i = %.4f, y_d = %.4f' % (self.pid_vel_y_.p, self.pid_vel_y_.i, self.pid_vel_y_.d), end="\n\r") 299 | 300 | # acceleration to lean angle 301 | acc_fwd = acc_target_x * self.cos_yaw_ + acc_target_y * self.sin_yaw_ 302 | acc_lft = -acc_target_x * self.sin_yaw_ + acc_target_y * self.cos_yaw_ 303 | 304 | # acc_fwd = 0 305 | # acc_lft = 0 306 | 307 | self.pitch_target_ef_ = np.arctan(acc_fwd/9.81) 308 | self.roll_target_ef_ = -np.arctan(acc_lft*np.cos(self.pitch_target_ef_)/9.81) 309 | print('roll target = %.4f, pitch target = %.4f' % (self.roll_target_ef_/np.pi*180, self.pitch_target_ef_/np.pi*180), end="\n\r") 310 | 311 | def attitude_control(self): 312 | # constraint roll pitch angle target 313 | # for debug 314 | #roll_target_ef_ = 0; 315 | #pitch_target_ef_ = 0; 316 | ang_ef_target_x = np.clip(self.roll_target_ef_, -20.0/180.0*np.pi, 20.0/180.0*np.pi) #roll can be 45 deg 317 | ang_ef_target_y = np.clip(self.pitch_target_ef_, -15.0/180.0*np.pi, 15.0/180.0*np.pi) 318 | 319 | ang_ef_error_x = self.wrap_180(ang_ef_target_x - self.roll_angle_) 320 | ang_ef_error_y = self.wrap_180(ang_ef_target_y - self.pitch_angle_) 321 | ang_ef_error_z = self.wrap_180(self.ang_ef_target_z_ - self.yaw_angle_) 322 | ang_ef_error_z = np.clip(ang_ef_error_z, -10/180*np.pi, 10/180*np.pi) 323 | 324 | # convert to body frame 325 | 326 | ang_bf_error = self.frame_ef_to_bf(ang_ef_error_x, ang_ef_error_y, ang_ef_error_z) 327 | ang_bf_error_x = ang_bf_error[0] 328 | ang_bf_error_y = ang_bf_error[1] 329 | ang_bf_error_z = ang_bf_error[2] 330 | # ang_bf_error_z = 0.0f; 331 | 332 | # roll angle to roll control 333 | self.pid_ang_roll_ = self.update_pid(ang_bf_error_x, self.pid_ang_roll_) 334 | self.differential_voltage_ = np.clip(self.pid_ang_roll_.p + self.pid_ang_roll_.i + self.pid_ang_roll_.d, -self.differential_voltage_max_, self.differential_voltage_max_) 335 | print('roll_p = %.4f, roll_i = %.4f, roll_d = %.4f' % (self.pid_ang_roll_.p, self.pid_ang_roll_.i, self.pid_ang_roll_.d), end="\n\r") 336 | 337 | self.pid_ang_pitch_ = self.update_pid(ang_bf_error_y, self.pid_ang_pitch_) 338 | self.mean_voltage_ = np.clip(self.pid_ang_pitch_.p + self.pid_ang_pitch_.i + self.pid_ang_pitch_.d, -self.mean_voltage_max_, self.mean_voltage_max_) 339 | print('pitch_p = %.4f, pitch_i = %.4f, pitch_d = %.4f' % (self.pid_ang_pitch_.p, self.pid_ang_pitch_.i, self.pid_ang_pitch_.d), end="\n\r") 340 | 341 | self.pid_ang_yaw_ = self.update_pid(ang_bf_error_z, self.pid_ang_yaw_) 342 | self.split_cycle_ = np.clip(self.pid_ang_yaw_.p + self.pid_ang_yaw_.i + self.pid_ang_yaw_.d, -self.split_cycle_max_, self.split_cycle_max_) 343 | print('yaw_p = %.4f, yaw_i = %.4f, yaw_d = %.4f' % (self.pid_ang_yaw_.p, self.pid_ang_yaw_.i, self.pid_ang_yaw_.d), end="\n\r") 344 | 345 | # update rate body frame targets 346 | # rate_bf_target_x = self.p_ang_roll_Kp_ * ang_bf_error_x 347 | # rate_bf_target_y = self.p_ang_pitch_Kp_ * ang_bf_error_y 348 | # rate_bf_target_z = self.p_ang_yaw_Kp_ * ang_bf_error_z 349 | 350 | #include roll and pitch rate required to account for precession of the desired attitude about the body frame yaw axes 351 | #rate_bf_target_x = rate_bf_target_x + ang_bf_error_y* gyro_z_; 352 | #rate_bf_target_y = rate_bf_target_y - ang_bf_error_x* gyro_z_; 353 | 354 | # run rate controller 355 | # self.rate_bf_to_roll(rate_bf_target_x) 356 | # self.rate_bf_to_pitch(rate_bf_target_y) 357 | # self.rate_bf_to_yaw(rate_bf_target_z) 358 | 359 | def wrap_180(self, angle): 360 | if (angle > 3*np.pi or angle < -3*np.pi): 361 | angle = np.fmod(angle,2*np.pi) 362 | if (angle > np.pi): 363 | angle = angle - 2*np.pi 364 | if (angle < - np.pi): 365 | angle = angle + 2*np.pi 366 | return angle; 367 | 368 | def rate_bf_to_roll(self, rate_target): 369 | rate_error = rate_target - self.gyro_x_ 370 | self.pid_rate_roll_ = self.update_pid(rate_error, self.pid_rate_roll_) 371 | self.differential_voltage_ = np.clip(self.pid_rate_roll_.p + self.pid_rate_roll_.i + self.pid_rate_roll_.d, -self.differential_voltage_max_, self.differential_voltage_max_) 372 | 373 | def rate_bf_to_pitch(self, rate_target): 374 | rate_error = rate_target - self.gyro_y_ 375 | self.pid_rate_pitch_ = self.update_pid(rate_error, self.pid_rate_pitch_) 376 | self.mean_voltage_ = np.clip(self.pid_rate_pitch_.p + self.pid_rate_pitch_.i + self.pid_rate_pitch_.d, -self.mean_voltage_max_, self.mean_voltage_max_) 377 | 378 | def rate_bf_to_yaw(self, rate_target): 379 | rate_error = rate_target - self.gyro_z_ 380 | self.pid_rate_yaw_ = self.update_pid(rate_error, self.pid_rate_yaw_) 381 | self.split_cycle_ = np.clip(self.pid_rate_yaw_.p + self.pid_rate_yaw_.i + self.pid_rate_yaw_.d, -self.split_cycle_max_, self.split_cycle_max_) 382 | 383 | def update_pid(self, error, pid_target, gyro = 0): 384 | integral = pid_target.integral + error*self.dt_ 385 | 386 | if (integral > pid_target.int_max): 387 | integral = pid_target.int_max 388 | elif (integral < -pid_target.int_max): 389 | integral = -pid_target.int_max 390 | 391 | if gyro == 0: 392 | derivative = (error - pid_target.old_error)/self.dt_ 393 | else: 394 | derivative = gyro 395 | pid_target.p = error*pid_target.Kp 396 | pid_target.i = integral*pid_target.Ki 397 | pid_target.d = derivative*pid_target.Kd 398 | 399 | # update all fields 400 | pid_target.old_error = error 401 | pid_target.integral = integral 402 | 403 | return pid_target 404 | 405 | def z_control(self): 406 | # position to rate 407 | pos_error_z = self.pos_target_z_ - self.altitude_ 408 | #pos_error_z = np.clip(pos_error_z, -0.2, 0.2) 409 | # self.pid_pos_z_ = self.update_pid(pos_error_z, self.pid_pos_z_) 410 | # voltage_out = self.pid_pos_z_.p + self.pid_pos_z_.i + self.pid_pos_z_.d 411 | # print('z_p = %.4f, z_i = %.4f, z_d = %.4f' % (self.pid_pos_z_.p, self.pid_pos_z_.i, self.pid_pos_z_.d), end="\n\r") 412 | vel_target_z = pos_error_z * self.p_pos_z_Kp_ 413 | 414 | # rate to acceleration 415 | vel_error_z = vel_target_z - self.velocity_z_ 416 | 417 | self.pid_vel_z_ = self.update_pid(vel_error_z, self.pid_vel_z_) 418 | voltage_out = self.pid_vel_z_.p + self.pid_vel_z_.i + self.pid_vel_z_.d 419 | print('z_p = %.4f, z_i = %.4f, z_d = %.4f' % (self.pid_vel_z_.p, self.pid_vel_z_.i, self.pid_vel_z_.d), end="\n\r") 420 | # acc_target_z = vel_error_z * self.p_vel_z_Kp_ 421 | 422 | # # acceleration to throttle 423 | # acc_error_z = acc_target_z - self.acceleration_z_ 424 | 425 | # self.pid_acc_z_ = self.update_pid(acc_error_z, self.pid_acc_z_) 426 | 427 | # voltage_out = self.pid_acc_z_.p + self.pid_acc_z_.i + self.pid_acc_z_.d 428 | voltage_out = voltage_out/(self.cos_pitch_*self.cos_roll_) 429 | 430 | self.voltage_amplitude_ = np.clip(voltage_out, 0, self.voltage_amplitude_max_ - self.hover_voltage_) 431 | self.voltage_amplitude_ += self.hover_voltage_ 432 | 433 | def frame_ef_to_bf(self, ef_x, ef_y, ef_z): 434 | bf = np.zeros([3],dtype=np.float64) 435 | 436 | bf[0] = ef_x - self.sin_pitch_*ef_z 437 | bf[1] = self.cos_roll_*ef_y + self.sin_roll_*self.cos_pitch_*ef_z 438 | bf[2] = -self.sin_roll_*ef_y + self.cos_pitch_*self.cos_roll_*ef_z 439 | 440 | return bf 441 | 442 | 443 | -------------------------------------------------------------------------------- /flappy/envs/fwmav/controllers/controller_maneuver.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class pid: 4 | def __init__(self): 5 | self.old_error = 0 6 | self.integral = 0 7 | self.int_max = 0 8 | self.Kp = 0 9 | self.Ki = 0 10 | self.Kd = 0 11 | self.p = 0 12 | self.i = 0 13 | self.d = 0 14 | 15 | class PIDController(): 16 | def __init__(self,dt): 17 | 18 | self.desired_accel_x_ = 0 19 | self.desired_accel_y_ = 0 20 | 21 | self.pos_target_x_ = 0 22 | self.pos_target_y_ = 0 23 | self.pos_target_z_ = 0.0 24 | 25 | self.ang_ef_target_z_ = 0 26 | 27 | # control here 28 | # xy postion to velocity 29 | self.p_pos_xy_Kp_ = 2 #1m error to 0.1m/s 30 | 31 | # z postion to velocity 32 | self.p_pos_z_Kp_ = 4#2 33 | # z velocity to acceleration 34 | # self.p_vel_z_Kp_ = 20 # need to test 35 | 36 | # z position PID controller 37 | # self.pid_pos_z_ = pid() 38 | # self.pid_pos_z_.old_error = 0 39 | # self.pid_pos_z_.integral = 0 40 | # self.pid_pos_z_.int_max = 5 41 | # self.pid_pos_z_.Kp = 10 42 | # self.pid_pos_z_.Ki = 5 43 | # self.pid_pos_z_.Kd = 8 44 | # self.pid_pos_z_.p = 0 45 | # self.pid_pos_z_.i= 0 46 | # self.pid_pos_z_.d = 0 47 | 48 | # z velocity PID controller 49 | self.pid_vel_z_ = pid() 50 | self.pid_vel_z_.old_error = 0 51 | self.pid_vel_z_.integral = 0 52 | self.pid_vel_z_.int_max = 5 53 | self.pid_vel_z_.Kp = 40#10 54 | self.pid_vel_z_.Ki = 0#4 55 | self.pid_vel_z_.Kd = 0.16#0.04 56 | self.pid_vel_z_.p = 0 57 | self.pid_vel_z_.i= 0 58 | self.pid_vel_z_.d = 0 59 | 60 | # z acceleration PID controller 61 | # self.pid_acc_z_ = pid() 62 | # self.pid_acc_z_.old_error = 0 63 | # self.pid_acc_z_.integral = 0 64 | # self.pid_acc_z_.int_max = 5 65 | # self.pid_acc_z_.Kp = 1 66 | # self.pid_acc_z_.Ki = 0 67 | # self.pid_acc_z_.Kd = 0.00001 68 | # self.pid_acc_z_.p = 0 69 | # self.pid_acc_z_.i= 0 70 | # self.pid_acc_z_.d = 0 71 | 72 | # x velocity PID controller (pitch) 73 | self.pid_vel_x_ = pid() 74 | self.pid_vel_x_.old_error = 0 75 | self.pid_vel_x_.integral = 0 76 | self.pid_vel_x_.int_max = 10 77 | self.pid_vel_x_.Kp = 2 78 | self.pid_vel_x_.Ki = 0#0.5 79 | self.pid_vel_x_.Kd = 0.01 80 | self.pid_vel_x_.p = 0 81 | self.pid_vel_x_.i= 0 82 | self.pid_vel_x_.d = 0 83 | self.acc_target_xy_max_ = 10 84 | 85 | # y velocity PID controller (roll) 86 | self.pid_vel_y_ = pid() 87 | self.pid_vel_y_.old_error = 0 88 | self.pid_vel_y_.integral = 0 89 | self.pid_vel_y_.int_max = 10 90 | self.pid_vel_y_.Kp = 2#1.75 91 | self.pid_vel_y_.Ki = 0#0.4 92 | self.pid_vel_y_.Kd = 0.004#0.002 93 | self.pid_vel_y_.p = 0 94 | self.pid_vel_y_.i= 0 95 | self.pid_vel_y_.d = 0 96 | 97 | # rpy angle to angular rate // control how fast angle converges 98 | self.p_ang_roll_Kp_ = 10 99 | self.p_ang_pitch_Kp_ = 10 100 | self.p_ang_yaw_Kp_ = 5 101 | 102 | # roll angular rate PID controller 103 | self.pid_ang_roll_ = pid() 104 | self.pid_ang_roll_.old_error = 0 105 | self.pid_ang_roll_.integral = 0 106 | self.pid_ang_roll_.int_max = 2 107 | self.pid_ang_roll_.Kp = 4#3 108 | self.pid_ang_roll_.Ki = 0#1.5 109 | self.pid_ang_roll_.Kd = 0.6#0.4 110 | self.pid_ang_roll_.p = 0 111 | self.pid_ang_roll_.i= 0 112 | self.pid_ang_roll_.d = 0 113 | 114 | # pitch angular rate PID controller 115 | self.pid_ang_pitch_ = pid() 116 | self.pid_ang_pitch_.old_error = 0 117 | self.pid_ang_pitch_.integral = 0 118 | self.pid_ang_pitch_.int_max = 2 119 | self.pid_ang_pitch_.Kp = 5 120 | self.pid_ang_pitch_.Ki = 0#1.5 121 | self.pid_ang_pitch_.Kd = 0.4 122 | self.pid_ang_pitch_.p = 0 123 | self.pid_ang_pitch_.i= 0 124 | self.pid_ang_pitch_.d = 0 125 | 126 | # yaw angular rate PID controller 127 | self.pid_ang_yaw_ = pid() 128 | self.pid_ang_yaw_.old_error = 0 129 | self.pid_ang_yaw_.integral = 0 130 | self.pid_ang_yaw_.int_max = 0.1 131 | self.pid_ang_yaw_.Kp = 0.4 132 | self.pid_ang_yaw_.Ki = 0#0.15 133 | self.pid_ang_yaw_.Kd = 0.001 134 | self.pid_ang_yaw_.p = 0 135 | self.pid_ang_yaw_.i= 0 136 | self.pid_ang_yaw_.d = 0 137 | 138 | # control voltage limit 139 | self.differential_voltage_max_ = 3 140 | self.mean_voltage_max_ = 3.5 141 | self.split_cycle_max_ = 0.15 142 | self.hover_voltage_ = 9.3 143 | self.voltage_amplitude_max_ = 18 144 | 145 | self.frequency_ = 34 146 | self.voltage_amplitude_ = 12 147 | self.differential_voltage_ = 0 148 | self.mean_voltage_ = 0 149 | self.split_cycle_ = 0 150 | 151 | # working trim 152 | self.roll_trim_ = 0.4; #voltage 153 | self.pitch_trim_ = -0.4; #voltage 154 | self.yaw_trim_ = -0.04; #split cycle 155 | 156 | # filter 157 | RC = 1/(2*np.pi*20) 158 | self.alpha = dt/(RC+dt) 159 | RC = 1/(2*np.pi*20) 160 | self.alpha_xyz = dt/(RC+dt) 161 | 162 | self.pos_current_x_ = 0 163 | self.pos_current_y_ = 0 164 | self.vel_current_x_ = 0 165 | self.vel_current_y_ = 0 166 | self.roll_angle_ = 0 167 | self.pitch_angle_ = 0 168 | self.yaw_angle_ = 0 169 | self.gyro_x_ = 0 170 | self.gyro_y_ = 0 171 | self.gyro_z_ = 0 172 | self.altitude_ = 0 173 | self.velocity_z_ = 0 174 | self.acceleration_z_ = 0 175 | 176 | 177 | def get_action(self, states, dt, reached): 178 | if reached == 1: 179 | self.p_pos_xy_Kp_ = 7 180 | self.dt_ = dt 181 | self.sensor_read(states) 182 | self.controller_run() 183 | self.add_trim() 184 | 185 | #print('differential_voltage_ = %.4f, mean_voltage_ = %.4f, split_cycle_ = %.4f' % (self.differential_voltage_, self.mean_voltage_, self.split_cycle_), end="\n\r") 186 | 187 | action_pid = np.zeros([4],dtype=np.float64) 188 | 189 | action_pid[0] = self.voltage_amplitude_ 190 | action_pid[1] = self.differential_voltage_ 191 | action_pid[2] = self.mean_voltage_ 192 | action_pid[3] = self.split_cycle_ 193 | 194 | return action_pid 195 | 196 | def controller_run(self): 197 | self.xy_control() 198 | self.attitude_control() 199 | self.z_control() 200 | 201 | def add_trim(self): 202 | self.differential_voltage_ = np.clip(self.differential_voltage_ + self.roll_trim_, -self.differential_voltage_max_, self.differential_voltage_max_) 203 | self.mean_voltage_ = np.clip(self.mean_voltage_ + self.pitch_trim_, -self.mean_voltage_max_, self.mean_voltage_max_) 204 | self.split_cycle_ = np.clip(self.split_cycle_ + self.yaw_trim_, -self.split_cycle_max_, self.split_cycle_max_) 205 | 206 | def sensor_read(self,states): 207 | raw_pos_current_x_ = states['body_positions'][3,0] + np.random.normal(0,0.0005) 208 | raw_pos_current_y_ = states['body_positions'][4,0] + np.random.normal(0,0.0005) 209 | raw_vel_current_x_ = states['body_spatial_velocities'][0,0] + np.random.normal(0,0.0005) 210 | raw_vel_current_y_ = states['body_spatial_velocities'][1,0] + np.random.normal(0,0.0005) 211 | raw_roll_angle_ = states['body_positions'][0,0] + np.random.normal(0,2/180*np.pi) 212 | raw_pitch_angle_ = states['body_positions'][1,0] + np.random.normal(0,2/180*np.pi) 213 | raw_yaw_angle_ = states['body_positions'][2,0] + np.random.normal(0,2/180*np.pi) 214 | raw_gyro_x_ = states['body_velocities'][0,0] + np.random.normal(0.01,5/180*np.pi) 215 | raw_gyro_y_ = states['body_velocities'][1,0] + np.random.normal(0.01,5/180*np.pi) 216 | raw_gyro_z_ = states['body_velocities'][2,0] + np.random.normal(0.01,5/180*np.pi) 217 | raw_altitude_ = states['body_positions'][5,0] + np.random.normal(0,0.0005) 218 | raw_velocity_z_ = states['body_spatial_velocities'][2,0] + np.random.normal(0,0.005) 219 | raw_acceleration_z_ = states['body_spatial_accelerations'][2,0] + np.random.normal(0,0.05) 220 | 221 | raw_pos_current_x_ = states['body_positions'][3,0] 222 | raw_pos_current_y_ = states['body_positions'][4,0] 223 | raw_vel_current_x_ = states['body_spatial_velocities'][0,0] 224 | raw_vel_current_y_ = states['body_spatial_velocities'][1,0] 225 | raw_roll_angle_ = states['body_positions'][0,0] 226 | raw_pitch_angle_ = states['body_positions'][1,0] 227 | raw_yaw_angle_ = states['body_positions'][2,0] 228 | raw_gyro_x_ = states['body_velocities'][0,0] 229 | raw_gyro_y_ = states['body_velocities'][1,0] 230 | raw_gyro_z_ = states['body_velocities'][2,0] 231 | raw_altitude_ = states['body_positions'][5,0] 232 | raw_velocity_z_ = states['body_spatial_velocities'][2,0] 233 | raw_acceleration_z_ = states['body_spatial_accelerations'][2,0] 234 | 235 | 236 | # filter z with 34 Hz low pass 237 | self.pos_current_x_ = self.pos_current_x_*(1-self.alpha_xyz) + raw_pos_current_x_*self.alpha_xyz 238 | self.pos_current_y_ = self.pos_current_y_*(1-self.alpha_xyz) + raw_pos_current_y_*self.alpha_xyz 239 | self.vel_current_x_ = self.vel_current_x_*(1-self.alpha_xyz) + raw_vel_current_x_*self.alpha_xyz 240 | self.vel_current_y_ = self.vel_current_y_*(1-self.alpha_xyz) + raw_vel_current_y_*self.alpha_xyz 241 | self.roll_angle_ = self.roll_angle_*(1-self.alpha) + raw_roll_angle_*self.alpha 242 | self.pitch_angle_ = self.pitch_angle_*(1-self.alpha) + raw_pitch_angle_*self.alpha 243 | self.yaw_angle_ = self.yaw_angle_*(1-self.alpha) + raw_yaw_angle_*self.alpha 244 | self.gyro_x_ = self.gyro_x_*(1-self.alpha) + raw_gyro_x_*self.alpha 245 | self.gyro_y_ = self.gyro_y_*(1-self.alpha) + raw_gyro_y_*self.alpha 246 | self.gyro_z_ = self.gyro_z_*(1-self.alpha) + raw_gyro_z_*self.alpha 247 | self.altitude_ = self.altitude_*(1-self.alpha_xyz) + raw_altitude_*self.alpha_xyz 248 | self.velocity_z_ = self.velocity_z_*(1-self.alpha_xyz) + raw_velocity_z_*self.alpha_xyz 249 | self.acceleration_z_ = self.acceleration_z_*(1-self.alpha_xyz) + raw_acceleration_z_*self.alpha_xyz 250 | 251 | # self.pos_current_x_ = raw_pos_current_x_ 252 | # self.pos_current_y_ = raw_pos_current_y_ 253 | # self.vel_current_x_ = raw_vel_current_x_ 254 | # self.vel_current_y_ = raw_vel_current_y_ 255 | # self.roll_angle_ = raw_roll_angle_ 256 | # self.pitch_angle_ = raw_pitch_angle_ 257 | # self.yaw_angle_ = raw_yaw_angle_ 258 | # self.gyro_x_ = raw_gyro_x_ 259 | # self.gyro_y_ = raw_gyro_y_ 260 | # self.gyro_z_ = raw_gyro_z_ 261 | 262 | # self.altitude_ = raw_altitude_ 263 | # self.velocity_z_ = raw_velocity_z_ 264 | # self.acceleration_z_ = raw_acceleration_z_ 265 | 266 | self.sin_roll_ = np.sin(self.roll_angle_) 267 | self.cos_roll_ = np.cos(self.roll_angle_) 268 | self.sin_pitch_ = np.sin(self.pitch_angle_) 269 | self.cos_pitch_ = np.cos(self.pitch_angle_) 270 | self.sin_yaw_ = np.sin(self.yaw_angle_) 271 | self.cos_yaw_ = np.cos(self.yaw_angle_) 272 | 273 | 274 | 275 | def xy_control(self): 276 | # desired acceleration to velocity 277 | desired_vel_x = self.desired_accel_x_ * self.dt_ 278 | desired_vel_y = self.desired_accel_y_ * self.dt_ 279 | 280 | # update xy controller 281 | # desired velocity to position 282 | self.pos_target_x_ = self.pos_target_x_ + desired_vel_x * self.dt_ 283 | self.pos_target_y_ = self.pos_target_y_ + desired_vel_y * self.dt_ 284 | 285 | # position to rate 286 | pos_error_x = self.pos_target_x_ - self.pos_current_x_ 287 | pos_error_y = self.pos_target_y_ - self.pos_current_y_ 288 | vel_target_x = self.p_pos_xy_Kp_ * pos_error_x 289 | vel_target_y = self.p_pos_xy_Kp_ * pos_error_y 290 | 291 | # rate to acceleration 292 | vel_error_x = vel_target_x - self.vel_current_x_ 293 | vel_error_y = vel_target_y - self.vel_current_y_ 294 | 295 | self.pid_vel_x_ = self.update_pid(vel_error_x, self.pid_vel_x_) 296 | acc_target_x = np.clip(self.pid_vel_x_.p+self.pid_vel_x_.i+self.pid_vel_x_.d, -self.acc_target_xy_max_, self.acc_target_xy_max_) 297 | #print('x_p = %.4f, x_i = %.4f, x_d = %.4f' % (self.pid_vel_x_.p, self.pid_vel_x_.i, self.pid_vel_x_.d), end="\n\r") 298 | self.pid_vel_y_ = self.update_pid(vel_error_y, self.pid_vel_y_) 299 | acc_target_y = np.clip(self.pid_vel_y_.p+self.pid_vel_y_.i+self.pid_vel_y_.d, -self.acc_target_xy_max_, self.acc_target_xy_max_) 300 | #print('y_p = %.4f, y_i = %.4f, y_d = %.4f' % (self.pid_vel_y_.p, self.pid_vel_y_.i, self.pid_vel_y_.d), end="\n\r") 301 | 302 | # acceleration to lean angle 303 | acc_fwd = acc_target_x * self.cos_yaw_ + acc_target_y * self.sin_yaw_ 304 | acc_lft = -acc_target_x * self.sin_yaw_ + acc_target_y * self.cos_yaw_ 305 | 306 | self.pitch_target_ef_ = np.arctan(acc_fwd/9.81) 307 | self.roll_target_ef_ = -np.arctan(acc_lft*np.cos(self.pitch_target_ef_)/9.81) 308 | #print('roll target = %.4f, pitch target = %.4f' % (self.roll_target_ef_/np.pi*180, self.pitch_target_ef_/np.pi*180), end="\n\r") 309 | 310 | def attitude_control(self): 311 | # constraint roll pitch angle target 312 | # for debug 313 | #roll_target_ef_ = 0; 314 | #pitch_target_ef_ = 0; 315 | ang_ef_target_x = np.clip(self.roll_target_ef_, -45.0/180.0*np.pi, 45.0/180.0*np.pi) #roll can be 45 deg 316 | ang_ef_target_y = np.clip(self.pitch_target_ef_, -30.0/180.0*np.pi, 30.0/180.0*np.pi) 317 | 318 | ang_ef_error_x = self.wrap_180(ang_ef_target_x - self.roll_angle_) 319 | ang_ef_error_y = self.wrap_180(ang_ef_target_y - self.pitch_angle_) 320 | ang_ef_error_z = self.wrap_180(self.ang_ef_target_z_ - self.yaw_angle_) 321 | ang_ef_error_z = np.clip(ang_ef_error_z, -10/180*np.pi, 10/180*np.pi) 322 | 323 | # convert to body frame 324 | 325 | ang_bf_error = self.frame_ef_to_bf(ang_ef_error_x, ang_ef_error_y, ang_ef_error_z) 326 | ang_bf_error_x = ang_bf_error[0] 327 | ang_bf_error_y = ang_bf_error[1] 328 | ang_bf_error_z = ang_bf_error[2] 329 | # ang_bf_error_z = 0.0f; 330 | 331 | # roll angle to roll control 332 | self.pid_ang_roll_ = self.update_pid(ang_bf_error_x, self.pid_ang_roll_) 333 | self.differential_voltage_ = np.clip(self.pid_ang_roll_.p + self.pid_ang_roll_.i + self.pid_ang_roll_.d, -self.differential_voltage_max_, self.differential_voltage_max_) 334 | #print('roll_p = %.4f, roll_i = %.4f, roll_d = %.4f' % (self.pid_ang_roll_.p, self.pid_ang_roll_.i, self.pid_ang_roll_.d), end="\n\r") 335 | 336 | self.pid_ang_pitch_ = self.update_pid(ang_bf_error_y, self.pid_ang_pitch_) 337 | self.mean_voltage_ = np.clip(self.pid_ang_pitch_.p + self.pid_ang_pitch_.i + self.pid_ang_pitch_.d, -self.mean_voltage_max_, self.mean_voltage_max_) 338 | #print('pitch_p = %.4f, pitch_i = %.4f, pitch_d = %.4f' % (self.pid_ang_pitch_.p, self.pid_ang_pitch_.i, self.pid_ang_pitch_.d), end="\n\r") 339 | 340 | self.pid_ang_yaw_ = self.update_pid(ang_bf_error_z, self.pid_ang_yaw_) 341 | self.split_cycle_ = np.clip(self.pid_ang_yaw_.p + self.pid_ang_yaw_.i + self.pid_ang_yaw_.d, -self.split_cycle_max_, self.split_cycle_max_) 342 | #print('yaw_p = %.4f, yaw_i = %.4f, yaw_d = %.4f' % (self.pid_ang_yaw_.p, self.pid_ang_yaw_.i, self.pid_ang_yaw_.d), end="\n\r") 343 | 344 | # update rate body frame targets 345 | # rate_bf_target_x = self.p_ang_roll_Kp_ * ang_bf_error_x 346 | # rate_bf_target_y = self.p_ang_pitch_Kp_ * ang_bf_error_y 347 | # rate_bf_target_z = self.p_ang_yaw_Kp_ * ang_bf_error_z 348 | 349 | #include roll and pitch rate required to account for precession of the desired attitude about the body frame yaw axes 350 | #rate_bf_target_x = rate_bf_target_x + ang_bf_error_y* gyro_z_; 351 | #rate_bf_target_y = rate_bf_target_y - ang_bf_error_x* gyro_z_; 352 | 353 | # run rate controller 354 | # self.rate_bf_to_roll(rate_bf_target_x) 355 | # self.rate_bf_to_pitch(rate_bf_target_y) 356 | # self.rate_bf_to_yaw(rate_bf_target_z) 357 | 358 | def wrap_180(self, angle): 359 | if (angle > 3*np.pi or angle < -3*np.pi): 360 | angle = np.fmod(angle,2*np.pi) 361 | if (angle > np.pi): 362 | angle = angle - 2*np.pi 363 | if (angle < - np.pi): 364 | angle = angle + 2*np.pi 365 | return angle; 366 | 367 | def rate_bf_to_roll(self, rate_target): 368 | rate_error = rate_target - self.gyro_x_ 369 | self.pid_rate_roll_ = self.update_pid(rate_error, self.pid_rate_roll_) 370 | self.differential_voltage_ = np.clip(self.pid_rate_roll_.p + self.pid_rate_roll_.i + self.pid_rate_roll_.d, -self.differential_voltage_max_, self.differential_voltage_max_) 371 | 372 | def rate_bf_to_pitch(self, rate_target): 373 | rate_error = rate_target - self.gyro_y_ 374 | self.pid_rate_pitch_ = self.update_pid(rate_error, self.pid_rate_pitch_) 375 | self.mean_voltage_ = np.clip(self.pid_rate_pitch_.p + self.pid_rate_pitch_.i + self.pid_rate_pitch_.d, -self.mean_voltage_max_, self.mean_voltage_max_) 376 | 377 | def rate_bf_to_yaw(self, rate_target): 378 | rate_error = rate_target - self.gyro_z_ 379 | self.pid_rate_yaw_ = self.update_pid(rate_error, self.pid_rate_yaw_) 380 | self.split_cycle_ = np.clip(self.pid_rate_yaw_.p + self.pid_rate_yaw_.i + self.pid_rate_yaw_.d, -self.split_cycle_max_, self.split_cycle_max_) 381 | 382 | def update_pid(self, error, pid_target, gyro = 0): 383 | integral = pid_target.integral + error*self.dt_ 384 | 385 | if (integral > pid_target.int_max): 386 | integral = pid_target.int_max 387 | elif (integral < -pid_target.int_max): 388 | integral = -pid_target.int_max 389 | 390 | if gyro == 0: 391 | derivative = (error - pid_target.old_error)/self.dt_ 392 | else: 393 | derivative = gyro 394 | pid_target.p = error*pid_target.Kp 395 | pid_target.i = integral*pid_target.Ki 396 | pid_target.d = derivative*pid_target.Kd 397 | 398 | # update all fields 399 | pid_target.old_error = error 400 | pid_target.integral = integral 401 | 402 | return pid_target 403 | 404 | def z_control(self): 405 | # position to rate 406 | pos_error_z = self.pos_target_z_ - self.altitude_ 407 | #pos_error_z = np.clip(pos_error_z, -0.2, 0.2) 408 | # self.pid_pos_z_ = self.update_pid(pos_error_z, self.pid_pos_z_) 409 | # voltage_out = self.pid_pos_z_.p + self.pid_pos_z_.i + self.pid_pos_z_.d 410 | # print('z_p = %.4f, z_i = %.4f, z_d = %.4f' % (self.pid_pos_z_.p, self.pid_pos_z_.i, self.pid_pos_z_.d), end="\n\r") 411 | vel_target_z = pos_error_z * self.p_pos_z_Kp_ 412 | 413 | # rate to acceleration 414 | vel_error_z = vel_target_z - self.velocity_z_ 415 | 416 | self.pid_vel_z_ = self.update_pid(vel_error_z, self.pid_vel_z_) 417 | voltage_out = self.pid_vel_z_.p + self.pid_vel_z_.i + self.pid_vel_z_.d 418 | #print('z_p = %.4f, z_i = %.4f, z_d = %.4f' % (self.pid_vel_z_.p, self.pid_vel_z_.i, self.pid_vel_z_.d), end="\n\r") 419 | # acc_target_z = vel_error_z * self.p_vel_z_Kp_ 420 | 421 | # # acceleration to throttle 422 | # acc_error_z = acc_target_z - self.acceleration_z_ 423 | 424 | # self.pid_acc_z_ = self.update_pid(acc_error_z, self.pid_acc_z_) 425 | 426 | # voltage_out = self.pid_acc_z_.p + self.pid_acc_z_.i + self.pid_acc_z_.d 427 | voltage_out = voltage_out/(self.cos_pitch_*self.cos_roll_) 428 | 429 | self.voltage_amplitude_ = np.clip(voltage_out, 0, self.voltage_amplitude_max_ - self.hover_voltage_) 430 | self.voltage_amplitude_ += self.hover_voltage_ 431 | 432 | def frame_ef_to_bf(self, ef_x, ef_y, ef_z): 433 | bf = np.zeros([3],dtype=np.float64) 434 | 435 | bf[0] = ef_x - self.sin_pitch_*ef_z 436 | bf[1] = self.cos_roll_*ef_y + self.sin_roll_*self.cos_pitch_*ef_z 437 | bf[2] = -self.sin_roll_*ef_y + self.cos_pitch_*self.cos_roll_*ef_z 438 | 439 | return bf 440 | 441 | 442 | --------------------------------------------------------------------------------