def startSensors(self, timestep):
""" Start sensors.
Instantiate objects and start up GPS, Gyro, IMU sensors.
For more details, refer to the Webots documentation.
Args:
timestep (int): time step of the current world.
"""
self.gps = GPS("gps")
self.gps.enable(timestep)
self.gyro = Gyro("gyro")
self.gyro.enable(timestep)
self.imu = InertialUnit("inertial unit")
self.imu.enable(timestep)
self.timestep = timestep
class BaseController():
""" The base controller class.
"""
def __init__(self, robot, lossOfThrust = 0):
""" Base controller __init__ method.
Initialize drone parameters here.
Args:
robot (webots controller object): Controller for the drone.
lossOfThrust (float): percent lost of thrust.
"""
# Initialize variables
self.robot = robot
self.timestep = 0
# intializa percent loss of thrust
self.lossOfThrust = lossOfThrust
# Define robot parameter
self.m = 0.4
self.d1x = 0.1122
self.d1y = 0.1515
self.d2x = 0.11709
self.d2y = 0.128
self.Ix = 0.000913855
self.Iy = 0.00236242
self.Iz = 0.00279965
# define constants
self.g = 9.81
self.ct = 0.00026
self.ctau = 5.2e-06
self.U1_max = 10
self.pi = 3.1415926535
# define H matrix for conversion from control input U to motor speeds
self.H_inv = self.ct*np.array([[1, 1, 1, 1],
[self.d1y, -self.d1y, self.d2y, -self.d2y],
[-self.d1x, -self.d1x, self.d2x, self.d2x],
[-self.ctau/self.ct, self.ctau/self.ct, self.ctau/self.ct, -self.ctau/self.ct]
])
self.H = np.linalg.inv(self.H_inv)
# define variables for speed calculations
self.xGPS_old = 0
self.yGPS_old = 0
self.zGPS_old = 0.099019
def startSensors(self, timestep):
""" Start sensors.
Instantiate objects and start up GPS, Gyro, IMU sensors.
For more details, refer to the Webots documentation.
Args:
timestep (int): time step of the current world.
"""
self.gps = GPS("gps")
self.gps.enable(timestep)
self.gyro = Gyro("gyro")
self.gyro.enable(timestep)
self.imu = InertialUnit("inertial unit")
self.imu.enable(timestep)
self.timestep = timestep
def getStates(self):
""" Get drone state.
The state of drone is 16 dimensional:
xGPS, yGPS, zGPS,
roll, pitch, yaw,
x_vel, y_vel, z_vel,
roll_rate, pitch_rate, yaw_rate
Returns:
np.array: x_t. information of 12 states.
"""
# Timestep returned by Webots is in ms, so we convert
delT = 1e-3*self.timestep
# Extract (X, Y, Z) coordinate from GPS
xGPS = self.gps.getValues()[0]
yGPS = -self.gps.getValues()[2]
zGPS = self.gps.getValues()[1]
# Find the rate of change in each axis, and store the current value of (X, Y, Z)
# as previous (X, Y, Z) which will be used in the next call
x_vel = (xGPS - self.xGPS_old)/delT
y_vel = (yGPS - self.yGPS_old)/delT
z_vel = (zGPS - self.zGPS_old)/delT
self.xGPS_old = xGPS
self.yGPS_old = yGPS
self.zGPS_old = zGPS
# Extract (roll, pitch, yaw) angle from imu
roll = self.imu.getRollPitchYaw()[0]
pitch = -self.imu.getRollPitchYaw()[1]
yaw = self.imu.getRollPitchYaw()[2]
# Extract (roll rate, pitch rate, yaw rate) angular velocity from imu
roll_rate = self.gyro.getValues()[0]
pitch_rate = -self.gyro.getValues()[2]
yaw_rate = self.gyro.getValues()[1]
x_t = np.array([xGPS, yGPS, zGPS, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_rate, pitch_rate, yaw_rate]).reshape(-1,1)
return x_t
def getMotorAll(self):
""" Get each motors' controller.
Returns:
list: Each motor's controller.
"""
frontLeftMotor = self.robot.getMotor('front left propeller')
frontRightMotor = self.robot.getMotor('front right propeller')
backLeftMotor = self.robot.getMotor('rear left propeller')
backRightMotor = self.robot.getMotor('rear right propeller')
return [frontLeftMotor, frontRightMotor, backLeftMotor, backRightMotor]
def initializeMotors(self):
""" Initialisze all motors speed to 0.
"""
[frontLeftMotor, frontRightMotor, backLeftMotor, backRightMotor] = self.getMotorAll()
frontLeftMotor.setPosition(float('inf'))
frontRightMotor.setPosition(float('inf'))
backLeftMotor.setPosition(float('inf'))
backRightMotor.setPosition(float('inf'))
self.motorsSpeed(0, 0, 0, 0)
def motorsSpeed(self, v1, v2, v3, v4):
""" Set each motors' speed.
Args:
v1, v2, v3, v4 (int): desired speed for each motor.
"""
[frontLeftMotor, frontRightMotor, backLeftMotor, backRightMotor] = self.getMotorAll()
frontLeftMotor.setVelocity(v1)
frontRightMotor.setVelocity(v2)
backLeftMotor.setVelocity(v3)
backRightMotor.setVelocity(v4)
def convertUtoMotorSpeed(self, U):
""" Convert control input to motor speed.
Args:
U (np.array): desired control input.
Returns:
np.array: rotorspeed. Desired rotor speed.
"""
w_squre = np.clip(np.matmul(self.H, U), 0, 576**2)
rotorspeed = np.sqrt(w_squre.flatten())
return rotorspeed
def setMotorsSpeed(self, motorspeed, motor_failure=0):
""" Set motor speed.
Args:
motorspeed (np.array): desired motor speed.
motor_failure (bool): True for motor failure, False otherwise.
"""
if motor_failure :
# print("--- Motor Failure ---")
factor = np.sqrt(1 - self.lossOfThrust)
self.motorsSpeed(float(motorspeed[0]) * factor, float(-motorspeed[1]), float(-motorspeed[2]), float(motorspeed[3]))
else:
self.motorsSpeed(float(motorspeed[0]), float(-motorspeed[1]), float(-motorspeed[2]), float(motorspeed[3]))
def move_forward():
motor_lst[1 + 0*3].setPosition(math.pi * -1 / 8)
motor_lst[1 + 0*3].setVelocity(1.0)
def rotate(angle):
for i in range(6):
motor_lst[0 + i*3].setPosition(angle)
motor_lst[0 + i*3].setVelocity(1.0)
camera = Camera("camera_d435i")
camera.enable(15)
print(camera.getSamplingPeriod())
camera.saveImage("~/test.png", 100)
gyro = Gyro("gyro")
gyro.enable(60)
inertial_unit = InertialUnit("inertial_unit")
inertial_unit.enable(60)
# Main loop:
# - perform simulation steps until Webots is stopping the controller
def default_low_pos():
for i in range(6):
motor_lst[0 + i*3].setPosition(0)
motor_lst[0 + i*3].setVelocity(1.0)
motor_lst[1 + i*3].setPosition(math.pi * 1 / 8)
motor_lst[1 + i*3].setVelocity(1.0)
print(lms291)
Lidar.enable(lms291, timestep)
Lidar.enablePointCloud(lms291)
lms291_yatayda = Lidar.getHorizontalResolution(lms291)
#print(lms291_yatayda)
#yatay=lms291_yatayda/2
#max_range=Lidar.getMaxRange(lms291)
#num_points=Lidar.getNumberOfPoints(lms291)
print("Lidar Başladı")
#araç üzeirnden gyro çekme
gyro = robot.getGyro("gyro")
Gyro.enable(gyro, timestep)
#araç üzerinden pususla çağırma
pusula = robot.getCompass("compass")
Compass.enable(pusula, timestep)
# motorların tagını getirir
#motorları getirir
solMotorİleri = robot.getMotor("front left wheel")
sağMotorİleri = robot.getMotor("front right wheel")
sağMotorGeri = robot.getMotor("back right wheel")
solMotorGeri = robot.getMotor("back left wheel")
#motorları hareket etirir
solMotorİleri.setPosition(float("inf"))
solMotorGeri.setPosition(float("inf"))
M_PI=np.pi
k_pitch_p=30.0
k_roll_p=50.0
k_vertical_p=3.0
k_vertical_thrust=68.5
k_vertical_offset=0.6
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
camera=robot.getCamera("camera")
Camera.enable(camera,timestep)
imu=InertialUnit("inertial unit")
imu.enable(timestep)
pusula=Compass("compass")
gyro=Gyro("gyro")
pusula.enable(timestep)
gyro.enable(timestep)
gps=GPS("gps")
gps.enable(timestep)
# motorların tagını getirir
#motorları getirir
solMotorİleri=robot.getMotor("front left propeller")
sağMotorİleri=robot.getMotor("front right propeller")
sağMotorGeri=robot.getMotor("rear right propeller")
solMotorGeri=robot.getMotor("rear left propeller")
#motorları hareket etirir
solMotorİleri.setPosition(float("inf"))
solMotorGeri.setPosition(float("inf"))
class BaseController():
def __init__(self, trajectory):
# Initialize variables
self.trajectory = trajectory
self.previousX = 0
self.previousY = 0
self.previousZ = 0
self.previousPsi = 0
self.previousXdotError = 0
self.integralXdotError = 0
def startSensors(self, timestep):
# Instantiate objects and start up GPS, Gyro, and Compass sensors
# For more details, refer to the Webots documentation
self.gps = GPS("gps")
self.gps.enable(timestep)
self.gyro = Gyro("gyro")
self.gyro.enable(timestep)
self.compass = Compass("compass")
self.compass.enable(timestep)
def getStates(self, timestep):
# Timestep returned by Webots is in ms, so we convert
delT = 0.001 * timestep
# Extract (X, Y) coordinate from GPS
position = self.gps.getValues()
X = position[0]
Y = position[1]
# Find the rate of change in each axis, and store the current value of (X, Y)
# as previous (X, Y) which will be used in the next call
Xdot = (X - self.previousX) / (delT + 1e-9)
self.previousX = X
Ydot = (Y - self.previousY) / (delT + 1e-9)
self.previousY = Y
XYdot = np.array([[Xdot], [Ydot]])
# Get heading angle and angular velocity
psi = wrapToPi(self.getBearingInRad())
angularVelocity = self.gyro.getValues()
psidot = angularVelocity[2]
# Get the rotation matrix (2x2) to convert velocities to the vehicle frame
rotation_mat = np.array([[np.cos(psi), -np.sin(psi)],
[np.sin(psi), np.cos(psi)]])
xdot = (np.linalg.inv(rotation_mat) @ XYdot)[0, 0]
ydot = (np.linalg.inv(rotation_mat) @ XYdot)[1, 0]
# Clamp xdot above 0 so we don't have singular matrices
xdot = clamp(xdot, 1e-5, np.inf)
return delT, X, Y, xdot, ydot, psi, psidot
def getBearingInRad(self):
# Get compass relative north vector
north = self.compass.getValues()
# Calculate vehicle's heading angle from north
rad = np.arctan2(north[1], north[0])
# Convert to vehicle's heading angle from x-axis
bearing = np.pi / 2.0 - rad
return bearing