Skip to content
GitLab
Commit 84a136b2 authored by Ibrahim's avatar Ibrahim
Browse files

physics: refactored apply_physics_torques variables names to calrify forces are in body frame

parent 19ab1d5f
Hide whitespace changes
Inline Side-by-side
control.py
View file @ 84a136b2
......@@ -39,109 +39,6 @@ class PIDController:
return self.k_p * self.err_p + self.k_i * self.err_i + self.k_d * self.err_d
@dataclass
class PositionController(PIDController):
vehicle: Multirotor
max_tilt: float = np.pi / 15
def __post_init__(self):
self.k_p = np.ones(3) * np.asarray(self.k_p)
self.k_i = np.ones(3) * np.asarray(self.k_i)
self.k_d = np.ones(3) * np.asarray(self.k_d)
self.err_p = np.zeros_like(self.k_p)
self.err_i = np.zeros_like(self.k_i)
self.err_d = np.zeros_like(self.k_d)
self.err = 0.
if self.max_err_i is None:
self.max_err_i = np.inf
def _step(self, reference, measurement):
roll, pitch, yaw = self.vehicle.orientation
err = reference - measurement
rot = np.asarray([
[np.cos(yaw), np.sin(yaw), 0],
[-np.sin(yaw), np.cos(yaw), 0],
[0, 0, 1]
])
# convert reference x/y to body frame, given yaw
# Using rotation matrix. For a positive yaw, the target x,y will appear
# rotated by a negative yaw in the body x/y axes
err_body = rot @ err
ctrl = super().step(reference=ref_body, measurement=measurement)
# ctrl[0] -> x dir -> pitch -> forward
# ctrl[1] -> y dir -> roll -> lateral
# ctrl[2] -> z dir -> thrust -> vertical
ctrl[0:2] = np.clip(ctrl[0:2], a_min=-self.max_tilt, a_max=self.max_tilt)
# ctrl[2] here is change in z-velocity i.e. acceleration
ctrl[2] = self.vehicle.params.mass * (
ctrl[2] / (np.cos(roll) * np.cos(pitch))
) + \
self.vehicle.weight
return ctrl # [delta pitch rate, delta roll rate, thrust force]
def step(self, reference, measurement):
xref = reference[0]
yref = reference[1]
psi = self.vehicle.orientation[2]
x = measurement[0]
y = measurement[1]
#print(x)
xdot = self.vehicle.velocity[0]
ydot = self.vehicle.velocity[1]
xerror = xref - x
yerror = yref - y
cosPsi = np.cos(psi)
sinPsi = np.sin(psi)
xErrorBodyFrame = xerror*cosPsi + yerror*sinPsi
yErrorBodyFrame = yerror*cosPsi - xerror*sinPsi
theta_des = (xErrorBodyFrame-xdot)*self.k_p[0] - xdot*self.k_d[0]
phi_des = ((yErrorBodyFrame-ydot)*self.k_p[1] - ydot*self.k_d[1])
theta_des = min(max(-np.pi / 15, theta_des), np.pi / 15)
phi_des = min(max(-np.pi / 15, phi_des), np.pi / 15)
thrust = self.vehicle.params.mass*(self.vehicle.simulation.g - self.k_d[2]*self.vehicle.velocity[2]-self.k_p[2]*(measurement[2]-reference[2]))
return theta_des, phi_des, thrust
@dataclass
class AttitudeController(PIDController):
vehicle: Multirotor
def step(self, reference, measurement):
ctrl = super().step(reference, measurement)
# torque = moment of inertia . angular_acceleration
return self.vehicle.params.inertia_matrix.dot(ctrl)
class PosAttController:
def __init__(self, ctrl_p: PositionController, ctrl_a: AttitudeController):
self.ctrl_p = ctrl_p
self.ctrl_a = ctrl_a
self.vehicle = self.ctrl_a.vehicle
assert self.ctrl_a.vehicle is self.ctrl_p.vehicle, "Vehicle instances different."
def reset(self):
self.ctrl_a.reset()
self.ctrl_p.reset()
def step(self, ref_pos, pos, ref_yaw, att):
# TODO: Att control is by angular velovity
pitch_rate, roll_rate, thrust = self.ctrl_p.step(ref_pos, pos)
ref_rate = np.asarray([roll_rate, pitch_rate, ref_yaw - att[2]])
torques = self.ctrl_a.step(ref_rate, self.vehicle.angular_rate)
return thrust, torques
# Altitude control
# Position Control (x,y) -> theta, phi
# Attitude Control (phi, theta, psi) -> delta phi, delta theta, delta psi
......
physics.py
View file @ 84a136b2
......@@ -81,9 +81,9 @@ def apply_forces_torques(
xI = x[0] # Inertial frame positions
yI = x[1]
zI = x[2]
ub = x[3] # linear velocity along body-frame-x-axis
vb = x[4] # linear velocity along body-frame-y-axis
wb = x[5] # linear velocity along body-frame-z-axis
ub = x[3] # linear velocity along body-frame-x-axis b1
vb = x[4] # linear velocity along body-frame-y-axis b2
wb = x[5] # linear velocity along body-frame-z-axis b3
phi = x[6] # Roll
theta = x[7] # Pitch
psi = x[8] # Yaw
......@@ -96,25 +96,27 @@ def apply_forces_torques(
cthe = np.cos(theta); sthe = np.sin(theta) # pitch
cpsi = np.cos(psi); spsi = np.sin(psi) # yaw
fx, fy, fz = forces
tx, ty, tz = torques
f1, f2, f3 = forces # in the body frame (b1, b2, b3)
t1, t2, t3 = torques
I = inertia_matrix
I_inv = inertia_matrix_inverse
# Calculate the derivative of the state matrix using EOM
xdot = np.zeros_like(x)
# Position
# velocity = dPosition (inertial) / dt (convert body velocity to inertial)
# Essentially = Rotation matrix (body to inertial) x body velocity
xdot[0] = cthe*cpsi*ub + (-cphi * spsi + sphi*sthe*cpsi) * vb + \
(sphi*spsi+cphi*sthe*cpsi) * wb # = xIdot
(sphi*spsi+cphi*sthe*cpsi) * wb # = xIdot
xdot[1] = cthe*spsi * ub + (cphi*cpsi+sphi*sthe*spsi) * vb + \
(-sphi*cpsi+cphi*sthe*spsi) * wb # = yIdot
(-sphi*cpsi+cphi*sthe*spsi) * wb # = yIdot
xdot[2] = (-sthe * ub + sphi*cthe * vb + cphi*cthe * wb) # = zIdot
# Velocity
xdot[3] = 1/mass * (fx) + g * sthe + r * vb - q * wb # = udot
xdot[4] = 1/mass * (fy) - g * sphi * cthe - r * ub + p * wb # = vdot
xdot[5] = 1/mass * (fz) - g * cphi * cthe + q * ub - p * vb # = wdot
# Acceleration = dVelocity (body frame) / dt
# External forces Gravity Coriolis effect
xdot[3] = 1/mass * (f1) + g * sthe + r * vb - q * wb # = udot
xdot[4] = 1/mass * (f2) - g * sphi * cthe - r * ub + p * wb # = vdot
xdot[5] = 1/mass * (f3) - g * cphi * cthe + q * ub - p * vb # = wdot
# Orientation
xdot[6] = p + (q*sphi + r*cphi) * sthe / cthe # = phidot
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment