Different result with explicit dynamics and Pybullet about gym-pybullet-drones

[JacopoPan]

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

gym-pybullet-drones/gym_pybullet_drones/envs/BaseAviary.py

Line 269 in beb4fff

def step(self,

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.

[shanki98]

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.

[JacopoPan]

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.

[shanki98]

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?

[JacopoPan]

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.

[shanki98]

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 !!

[JacopoPan]

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).