Comments (7)
Hi @shanki98,
try and use ``` before and after a block of code to properly display it in markdown
For example
x = 0
print(x)
Physics.DYN
and Physics.PYB
should work reasonably similarly (minus numerical approximation) at the default 240Hz stepping frequency. It might be possible that they give you different results at much lower or higher frequencies but I don't think it should be the case with 500Hz.
Assuming that your top level script is properly set up, all the logic you are concerned to is run by the step in BaseAviary
My suggestion is to make sure that your control and simulation frequencies match/are properly aligned. If you are willing to share your code, I will have a look at it.
from gym-pybullet-drones.
Thanks for getting back to me I can share my code.
import numpy as np
import matplotlib.pyplot as plt
import math
import time
import IPython
from Reference_CF import CFreference
from gym_pybullet_drones.envs.BaseAviary import DroneModel, Physics, BaseAviary
from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary
from gym_pybullet_drones.control.BaseControl import BaseControl
from utils import *
##### To remove any existing registrations of environments to avoid re-registry
import gym
env_dict = gym.envs.registration.registry.env_specs.copy()
for i in env_dict:
if 'ctrl-aviary-v0' or 'takeoff-aviary-v0' or 'flock-aviary-v0' in i:
print("Remove {} from registry".format(i))
del gym.envs.registration.registry.env_specs[i]
INIT_XYZS = np.array([[0,0,0]]);
INIT_RPYS = np.array([[0,0,0]]);
env = CtrlAviary(drone_model=DroneModel.CF2X, # See DroneModel Enum class for other quadcopter models
num_drones=1, # Number of drones
initial_xyzs=INIT_XYZS, # Initial XYZ position of the drone
neighbourhood_radius=np.inf, # Distance at which drones are considered neighbors, only used for multiple drones
physics=Physics.PYB, # Choice of (PyBullet) physics implementation
freq=500, # Stepping frequency of the simulation
aggregate_phy_steps=1, # Number of physics updates within each call to BaseAviary.step()
gui=True, # Whether to display PyBullet's GUI, only use this for debbuging
record=False, # Whether to save a .mp4 video (if gui=True) or .png frames (if gui=False) in gym-pybullet-drones/files/, see script /files/ffmpeg_png2mp4.sh for encoding
obstacles= False, # Whether to add obstacles to the environment
user_debug_gui=False, # Whether to use addUserDebugLine and addUserDebugParameter calls (it can slow down the GUI)
initial_rpys=np.array([0,0,0]).reshape(1,3))
parameters = BaseControl(env);
class CFParameters:
def __init__(self):
self.m = 0.027;
self.L = 0.039730;
self.KF = parameters.KF;
self.KM = parameters.KM;
self.MAX_THRUST = parameters.MAX_THRUST;
self.MAX_XY_TORQUE = parameters.MAX_XY_TORQUE;
self.MAX_Z_TORQUE = parameters.MAX_Z_TORQUE;
self.I = np.diag([0.00001395,0.00001436,0.00002173]);
self.Pos_ctrl_HZ = 100;
self.Att_ctrl_HZ = 250;
self.Rate_ctrl_HZ = 500;
self.MIN_PWM = 20000;
self.MAX_PWM = 65535;
self.CF_vel = 1.2;#m/s
g = 9.8;
cf2x = CFParameters();
Ts_Pos = 1/cf2x.Pos_ctrl_HZ;
Ts_att = 1/cf2x.Att_ctrl_HZ;
Ts_rate = 1/cf2x.Rate_ctrl_HZ;
Duration_sec = 15;
#Duration_sec = 40;
T_out = np.arange(0,Duration_sec,Ts_Pos);
T_rate = np.arange(0,Duration_sec,Ts_rate);
T_rpref = np.arange(0,Duration_sec,15/4500);
#T_rpref = np.arange(0,Duration_sec,Duration_sec/12000);
#Traj = CFreference(Duration_sec=40);
Traj = CFreference(Duration_sec=15);
Time = 0.0;
Traj.Linear(Time);
#Traj.Helical(Time);
#Traj.Circular(Time,Duration_sec=40);
k_p_x =1.59;
k_i_x =5.73;
k_p_y =-1.59;
k_i_y =-5.73;
k_p_z = 17000;
k_i_z = 0;
k_d_z = 8900;
k_pp_rate = 12;
k_ip_rate = 0;
k_pq_rate = 12;
k_iq_rate = 0;
k_pr_rate = 3;
k_p_phi = 4500;
k_p_theta= 4500;
k_p_psi=750;# I Retuned it a little more (350 was original)
k_i_psi=16.7;
Thrust = list();
def errortobframe(Traj,x,y,psi,indx):
x_ref = Traj.x_ref[indx,0];
y_ref = Traj.x_ref[indx,1];
#Error Projected onto the Body-fixed frame
e_x_b = (x_ref-x)*np.cos(psi)+(y_ref-y)*np.sin(psi);
e_y_b = -(x_ref-x)*np.sin(psi )+(y_ref-y)*np.cos(psi);
return e_x_b,e_y_b
def XYpositiontracker(e_x_b,e_y_b,x_dot,y_dot,psi,integral_pos_e,Ts):
x_b_dot = x_dot*np.cos(psi) + y_dot*np.sin(psi);
y_b_dot = -x_dot*np.sin(psi) + y_dot*np.cos(psi);
integral_pos_e = integral_pos_e+[(e_x_b - x_b_dot)*Ts,(e_y_b - y_b_dot)*Ts];
###Desired Phi and Desired Theta
des_theta = k_p_x*(e_x_b-x_b_dot) + k_i_x*(integral_pos_e[0]);
des_phi = k_p_y*(e_y_b-y_b_dot) + k_i_y*(integral_pos_e[1]);
## Max_Roll_Pitch = 30 degrees
des_phi = np.clip(des_phi,(-np.pi/6),(np.pi/6));
des_theta = np.clip(des_theta,(-np.pi/6),(np.pi/6));
return des_phi,des_theta,integral_pos_e
def Attitudecontrol(des_rp,phi,theta,integral_att_e,Ts):
des_rp = np.squeeze(des_rp);
integral_att_e = integral_att_e + [(des_rp[0]-phi)*Ts,(des_rp[1]-theta)*Ts];
p_des = k_pp_rate*(des_rp[0]-phi)+k_ip_rate*integral_att_e[0];
q_des = k_pq_rate*(des_rp[1]-theta)+k_iq_rate*integral_att_e[1];
# p_des = np.clip(p_des,(-np.radians(200)),np.radians(200));
# q_des = np.clip(p_des,(-np.radians(200)),np.radians(200));
return p_des,q_des,integral_att_e
def Altitudecontrol(Traj,z,z_dot,indx,integral_alt_e,Ts):
z_ref = Traj.x_ref[indx,2];
des_z_dot = Traj.xdot_ref[indx,2];
integral_alt_e = integral_alt_e + (z_ref - z)*Ts;
del_omega = k_d_z *(des_z_dot - z_dot)+k_p_z*(z_ref - z) + k_i_z*(integral_alt_e);
del_omega = np.clip(del_omega,-20000,15000);
omega_e = 38726.73;
del_omega = omega_e + del_omega;
RPM_T = np.floor(0.2685*del_omega + 4070.3);
RPM_T = np.clip(RPM_T,0,env.MAX_RPM);
F = 4*cf2x.KF*(RPM_T**2);
F = np.clip(F,0,cf2x.MAX_THRUST);
Thrust.append(F);
return del_omega,integral_alt_e
def yawpositioncontrol(Traj,psi,indx):
des_psi = Traj.yaw_ref[indx,0];
r_des = k_pr_rate*(des_psi - psi);
# r_des = np.clip(r_des,-(math.radians(200)),(math.radians(200)));
return r_des
def Ratecontrol(des_p,des_q,des_r,omega,p,q,r,integral_psir_e,Ts):
del_theta =(k_p_theta*(des_q - q));
del_phi = (k_p_phi*(des_p - p));
integral_psir_e = integral_psir_e + (des_r-r)*Ts
del_psi = k_p_psi*(des_r - r)+k_i_psi*(integral_psir_e);
PWM_1 = omega + del_phi/2 - del_theta/2 - del_psi;
PWM_2 = omega + del_phi/2 + del_theta/2 + del_psi;
PWM_3 = omega - del_phi/2 + del_theta/2 - del_psi;
PWM_4 = omega - del_phi/2 - del_theta/2 + del_psi;
PWM_1 = np.clip(PWM_1,0,cf2x.MAX_PWM);
PWM_2 = np.clip(PWM_2,0,cf2x.MAX_PWM);
PWM_3 = np.clip(PWM_3,0,cf2x.MAX_PWM);
PWM_4 = np.clip(PWM_4,0,cf2x.MAX_PWM);
##Convert PWM to RPM
RPM_1 = np.floor(0.2685*PWM_1 + 4070.3);
RPM_2 = np.floor(0.2685*PWM_2 + 4070.3);
RPM_3 = np.floor(0.2685*PWM_3 + 4070.3);
RPM_4 = np.floor(0.2685*PWM_4 + 4070.3);
M = np.array([[(cf2x.L*cf2x.KF/math.sqrt(2))*(-(RPM_1**2)-(RPM_2**2)+(RPM_3**2)+(RPM_4**2))],
[((cf2x.L*cf2x.KF)/math.sqrt(2))*(-(RPM_1**2)+(RPM_2**2)+(RPM_3**2)-(RPM_4**2))],
[cf2x.KM*(-(RPM_1**2)+(RPM_2**2)-(RPM_3**2)+(RPM_4**2))]]);
M[0]=np.clip(M[0],-cf2x.MAX_XY_TORQUE,cf2x.MAX_XY_TORQUE);
M[1]=np.clip(M[1],-cf2x.MAX_XY_TORQUE,cf2x.MAX_XY_TORQUE);
M[2]=np.clip(M[2],-cf2x.MAX_Z_TORQUE,cf2x.MAX_Z_TORQUE);
RPM_1 = np.clip(RPM_1,0,env.MAX_RPM);
RPM_2 = np.clip(RPM_2,0,env.MAX_RPM);
RPM_3 = np.clip(RPM_3,0,env.MAX_RPM);
RPM_4 = np.clip(RPM_4,0,env.MAX_RPM);
return RPM_1,RPM_2,RPM_3,RPM_4,integral_psir_e
PYB_CLIENT = env.getPyBulletClient();
integral_alt_e = 0;
integral_pos_e = np.zeros(shape = (2));
integral_att_e = np.zeros(shape = (2));
integral_psir_e = 0;
ref_rp = np.zeros(shape=(2,len(T_out)*3));
obs = env.reset();
xs = list()
ys = list()
zs = list()
phis = list()
thetas = list()
psis = list()
START = time.time();
Duration = 15;
#Duration = 40;
RPM = math.sqrt((0.027*9.8)/(4*cf2x.KF));
for _ in range(Duration*500):
if _ == 0:
idx = 0;
rp_idx = 0;
x = obs['0']['state'][0];
y = obs['0']['state'][1];
z = obs['0']['state'][2];
x_dot = obs['0']['state'][10];
y_dot = obs['0']['state'][11];
z_dot = obs['0']['state'][12];
phi = obs['0']['state'][7];
theta = obs['0']['state'][8];
psi = obs['0']['state'][9];
p = obs['0']['state'][13];
q = obs['0']['state'][14];
r = obs['0']['state'][15];
e_x_b,e_y_b = errortobframe(Traj,x,y,psi,idx);
ref_rp[0,[rp_idx]],ref_rp[1,[rp_idx]],integral_pos_e= XYpositiontracker(e_x_b,e_y_b,x_dot,y_dot,psi,integral_pos_e,Ts_Pos);
des_r = yawpositioncontrol(Traj,psi,idx);
des_p,des_q,integral_att_e = Attitudecontrol(ref_rp[:,[rp_idx]],phi,theta,integral_att_e,Ts_att);
Omega,integral_alt_e = Altitudecontrol(Traj,z,z_dot,idx,integral_alt_e,Ts_rate);
RPM_1,RPM_2,RPM_3,RPM_4,integral_psir_e = Ratecontrol(des_p,des_q,des_r,Omega,p,q,r,integral_psir_e,Ts_rate);
Input = dict({'0':[RPM_1,RPM_2,RPM_3,RPM_4]});
obs, reward, done, info = env.step(Input);
xs.append(obs['0']['state'][0]);
ys.append(obs['0']['state'][1]);
zs.append(obs['0']['state'][2]);
phis.append(obs['0']['state'][7]);
thetas.append(obs['0']['state'][8]);
psis.append(obs['0']['state'][9]);
env._showDroneLocalAxes(0)
env.render()
if done: obs = env.reset()
#### Sync the simulation ###########################################################################
sync(_,START, env.TIMESTEP)
Time = Time + env.TIMESTEP;
else:
if ( _ %5==0):
x = obs['0']['state'][0];
y = obs['0']['state'][1];
z = obs['0']['state'][2];
x_dot = obs['0']['state'][10];
y_dot = obs['0']['state'][11];
z_dot = obs['0']['state'][12];
phi = obs['0']['state'][7];
theta = obs['0']['state'][8];
psi = obs['0']['state'][9];
p = obs['0']['state'][13];
q = obs['0']['state'][14];
r = obs['0']['state'][15];
idx+=1;
rp_idx+=1;
e_x_b,e_y_b = errortobframe(Traj,x,y,psi,idx);
ref_rp[0,[rp_idx]],ref_rp[1,[rp_idx]],integral_pos_e= XYpositiontracker(e_x_b,e_y_b,x_dot,y_dot,psi,integral_pos_e,Ts_Pos);
des_r = yawpositioncontrol(Traj,psi,idx);
des_p,des_q,integral_att_e = Attitudecontrol(ref_rp[:,[rp_idx]],phi,theta,integral_att_e,Ts_att);
Omega,integral_alt_e = Altitudecontrol(Traj,z,z_dot,idx,integral_alt_e,Ts_rate);
RPM_1,RPM_2,RPM_3,RPM_4,integral_psir_e = Ratecontrol(des_p,des_q,des_r,Omega,p,q,r,integral_psir_e,Ts_rate);
Input = dict({'0':[RPM_1,RPM_2,RPM_3,RPM_4]});
obs, reward, done, info = env.step(Input);
xs.append(obs['0']['state'][0]);
ys.append(obs['0']['state'][1]);
zs.append(obs['0']['state'][2]);
phis.append(obs['0']['state'][7]);
thetas.append(obs['0']['state'][8]);
psis.append(obs['0']['state'][9]);
env._showDroneLocalAxes(0)
env.render()
if done: obs = env.reset()
#### Sync the simulation ###########################################################################
sync(_,START, env.TIMESTEP)
Time = Time + env.TIMESTEP;
elif (_ %2 == 0):
phi = obs['0']['state'][7];
theta = obs['0']['state'][8];
rp_idx+=1;
ref_rp[:,[rp_idx]]=ref_rp[:,[rp_idx-1]];
des_p,des_q,integral_att_e = Attitudecontrol(ref_rp[:,[rp_idx]],phi,theta,integral_att_e,Ts_att);
z = obs['0']['state'][2];
z_dot = obs['0']['state'][12];
psi = obs['0']['state'][9];
p = obs['0']['state'][13];
q = obs['0']['state'][14];
r = obs['0']['state'][15];
Omega,integral_alt_e = Altitudecontrol(Traj,z,z_dot,idx,integral_alt_e,Ts_rate);
RPM_1,RPM_2,RPM_3,RPM_4,integral_psir_e = Ratecontrol(des_p,des_q,des_r,Omega,p,q,r,integral_psir_e,Ts_rate);
Input = dict({'0':[RPM_1,RPM_2,RPM_3,RPM_4]});
obs, reward, done, info = env.step(Input);
xs.append(obs['0']['state'][0]);
ys.append(obs['0']['state'][1]);
zs.append(obs['0']['state'][2]);
phis.append(obs['0']['state'][7]);
thetas.append(obs['0']['state'][8]);
psis.append(obs['0']['state'][9]);
env._showDroneLocalAxes(0)
env.render()
if done: obs = env.reset()
#### Sync the simulation ###########################################################################
sync(_,START, env.TIMESTEP)
Time = Time + env.TIMESTEP;
else:
z = obs['0']['state'][2];
z_dot = obs['0']['state'][12];
psi = obs['0']['state'][9];
p = obs['0']['state'][13];
q = obs['0']['state'][14];
r = obs['0']['state'][15];
Omega,integral_alt_e = Altitudecontrol(Traj,z,z_dot,idx,integral_alt_e,Ts_rate);
RPM_1,RPM_2,RPM_3,RPM_4,integral_psir_e = Ratecontrol(des_p,des_q,des_r,Omega,p,q,r,integral_psir_e,Ts_rate);
Input = dict({'0':[RPM_1,RPM_2,RPM_3,RPM_4]});
obs, reward, done, info = env.step(Input);
xs.append(obs['0']['state'][0]);
ys.append(obs['0']['state'][1]);
zs.append(obs['0']['state'][2]);
phis.append(obs['0']['state'][7]);
thetas.append(obs['0']['state'][8]);
psis.append(obs['0']['state'][9]);
env._showDroneLocalAxes(0)
env.render()
if done: obs = env.reset()
#### Sync the simulation ###########################################################################
sync(_,START, env.TIMESTEP)
Time = Time + env.TIMESTEP;
env.close()
Basically here, XYPositiontracker, errortobframe and yaw position controller work at 100 Hz, the Attitude controller works at 250 Hz and Rate controller works at 500 Hz. I convert PWM signals to RPM commands and send them to the drone.
from gym-pybullet-drones.
I think I see what might lead you to different results. In the current commit, obs['0']['state'][13:16]
contains the component of the angular velocity, not the turn rates, for Physics.PYB
and 0s for Physics.DYN
You could try to replace the occurrences of these lines
p = obs['0']['state'][13];
q = obs['0']['state'][14];
r = obs['0']['state'][15];
with just
p = 0
q = 0
r = 0
or something like (but you'll need to store the previous rpy's)
p = (obs['0']['state'][7]-last_phi)/timestep
q = (obs['0']['state'][8]-last_theta)/timestep
r = (obs['0']['state'][9]-last_psi)/timestep
If nothing else, the behaviour should be the same then.
from gym-pybullet-drones.
Hey Jacopo,
thanks for looking into my code. I really appreciate it. I corrected the p,q,r though I'm getting an oscillatory response. It is better than the previous one. Can you tell me if Wx, Wy, Wz are angular velocities measured in terms of body-frame or inertial frame?
from gym-pybullet-drones.
I would try to tune the PID coefficient first, then. The angular velocity values are only stored/available when using Physics.PYB
, not Physics.DYN
. They are those returned by PyBullet's getBaseVelocity
method.
from gym-pybullet-drones.
Finally, It worked after retuning the PID coefficients a little bit. But I should say the stepSimulation of pybullet did give a different response than the explicit dynamics before I corrected it. (I checked this by giving the same input and observing the response of the system in both cases for a single step).
For p,q,r, I did :
quat = env.quat[0,:];
wx,wy,wz = env.ang_v[0,:]
rotation = np.array(pb.getMatrixFromQuaternion(quat)).reshape(3,3)
p,q,r = np.dot(rotation.transpose(),np.array([wx,wy,wz]))
This gave me the p,q,r in the inertial frame. Then after just retuning the PID coefficients a liittle bit, I got convergence!
Thanks a lot for your help !!
from gym-pybullet-drones.
I'm glad you managed to have your controller to work. You should keep in mind that env.ang_v[0,:]
is only written for Physics.PYB
. I believe that's were your implementation differs (minus, possibly, numerical approximation in near zero values).
from gym-pybullet-drones.
Related Issues (20)
- Units HOT 1
- High frequency in RPMs when include action buffer in observation space can couse problems in real hardware HOT 2
- Why might my rewards be inversely proportional to the target height in the HoverAviary environment? HOT 2
- Error while running velocity.py and fly.py examples HOT 1
- ray 1.9 error while installing gym-pybullet-drones HOT 1
- Sim2real transfer for betaflight HOT 2
- Request to Migrate from Master to Main Branch for Improved Stability HOT 3
- learn.py: Incompatibility with gym env despite having stable_baselines3 version 2.x
- Motor Delay HOT 2
- Simulation environment capabilities/scenario assistance for fault detection HOT 1
- one_d_rpm breaks with different masses for HoverAviary HOT 4
- Create custom simulation environment to train UAV collision avoidance HOT 6
- Issues with HoverAviary: Navigation and Plotting HOT 19
- Occupancy grid and disparity map from depth image HOT 1
- Questions about camera Settings HOT 4
- No module named 'gym_pybullet_drones' HOT 2
- Deviations in the actual CTRL_FREQ for the training loop HOT 2
- M2 Mac Failed to build pybullet HOT 1
- Fixed Wings? HOT 1
- Episode truncated and is still showing mean_episode_length in the printed logs as 242.
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from gym-pybullet-drones.