def latlon_diff(lat_ref, lon_ref, lat, lon):
"""Calculate difference in North and East from two GPS coordinates
Parameters
----------
lat_ref : float
Latitude of origin (decimal format)
lon_ref : float
Longitude of origin (decimal format)
offset_N : float
Offset in North direction (meters)
offset_E : float
Offset in East direction (meters)
lat :
lon :
Returns
-------
"""
d_lon = lon - lon_ref
d_lat = lat - lat_ref
dist_N = deg2rad(d_lat) * EARTH_RADIUS_M
dist_E = deg2rad(d_lon) * EARTH_RADIUS_M * cos(deg2rad(lat))
return (dist_N, dist_E)
def __init__(self):
self.arm_length = 0.4
self.arm_config = "x"
self.pose = [
0.0, 0.0, 10.0,
deg2rad(10.0),
deg2rad(20.0),
deg2rad(30.0)
]
def test_sandbox(self):
p_I_C = np.array([1.0, 2.0, 3.0])
p_G_I = np.array([1.0, 0.0, 0.0])
rpy_IG = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)])
q_IG = euler2quat(rpy_IG)
C_IG = C(q_IG)
print(p_G_I + np.dot(C_IG, p_I_C))
def test_update(self):
setpoints = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(10.0), 0.5])
dt = 0.001
for i in range(1000):
actual = np.array([
self.quadrotor.states[0], self.quadrotor.states[1],
self.quadrotor.states[2], self.quadrotor.states[8]
])
motor_inputs = self.attitude_controller.update(
setpoints, actual, dt)
self.quadrotor.update(motor_inputs, dt)
def test_observed_features(self):
# Setup camera model
K = camera_intrinsics(554.25, 554.25, 320.0, 320.0)
camera_model = PinholeCameraModel(640, 640, K)
# Setup random 3d features
feature = np.array([[0.0], [0.0], [10.0]])
# Test
rpy = np.array([deg2rad(0.0), deg2rad(-10.0), 0.0])
t = np.array([0.0, 0.0, 0.0])
observed = camera_model.observed_features(feature, rpy, t)
# Assert
self.assertTrue(len(observed) > 0)
def update(self, setpoints, actual, dt):
""" Update attitude controller
Args:
setpoint
actual
dt
Returns:
outputs
"""
self.dt += dt
if self.dt < 0.001:
return self.outputs
# update yaw error
actual_yaw = rad2deg(actual[2])
setpoint_yaw = rad2deg(setpoints[2])
error_yaw = setpoint_yaw - actual_yaw
if error_yaw > 180.0:
error_yaw -= 360.0
elif error_yaw < -180.0:
error_yaw += 360.0
error_yaw = deg2rad(error_yaw)
# roll pitch yaw
r = self.roll_controller.update(setpoints[0], actual[0], self.dt)
p = self.pitch_controller.update(setpoints[1], actual[1], self.dt)
y = self.yaw_controller.update(error_yaw, 0.0, self.dt)
# thrust
max_thrust = 5.0
t = max_thrust * setpoints[3] # convert relative to true thrust
t = max_thrust if t > max_thrust else t # limit thrust
t = 0.0 if t < 0 else t # limit thrust
# map roll, pitch, yaw and thrust to motor outputs
outputs = [0.0, 0.0, 0.0, 0.0]
outputs[0] = -p - y + t
outputs[1] = -r + y + t
outputs[2] = p - y + t
outputs[3] = r + y + t
# limit outputs
for i in range(4):
if outputs[i] > max_thrust:
outputs[i] = max_thrust
elif outputs[i] < 0.0:
outputs[i] = 0.0
# keep track of outputs
self.outputs = outputs
self.dt = 0.0
return outputs
def test_plot_elbow_manipulator(self):
# Link angles
link1_theta = 20.0
link2_theta = 10.0
# Create base frame
rpy = [0.0, 0.0, 0.0]
R_BG = euler2rot([deg2rad(i) for i in rpy], 321)
t_G_B = np.array([2.0, 1.0, 0.0])
T_GB = np.array([[R_BG[0, 0], R_BG[0, 1], R_BG[0, 2], t_G_B[0]],
[R_BG[1, 0], R_BG[1, 1], R_BG[1, 2], t_G_B[1]],
[R_BG[2, 0], R_BG[2, 1], R_BG[2, 2], t_G_B[2]],
[0.0, 0.0, 0.0, 1.0]])
# Create DH Transforms
T_B1 = dh_transform(deg2rad(link1_theta), 0.0, 1.0, 0.0)
T_12 = dh_transform(deg2rad(link2_theta), 0.0, 1.0, 0.0)
# Create Transforms
T_G1 = np.dot(T_GB, T_B1)
T_G2 = np.dot(T_GB, np.dot(T_B1, T_12))
# Transform from origin to end-effector
links = []
links.append(T_G1)
links.append(T_G2)
# Plot first link
# debug = True
debug = False
if debug:
plt.figure()
plt.plot([T_GB[0, 3], links[0][0, 3]],
[T_GB[1, 3], links[0][1, 3]])
plt.plot([links[0][0, 3], links[1][0, 3]],
[links[0][1, 3], links[1][1, 3]])
obs = np.array([1.0, 1.0, 0.0, 1.0])
obs_end = np.dot(T_G2, obs)
plt.plot(obs_end[0], obs_end[1], marker="x")
plt.xlim([0.0, 6.0])
plt.ylim([0.0, 6.0])
plt.show()
def plot_3d_cube(ax, width, origin, orientation):
""" Plot 3D cube
Parameters
----------
ax : matplotlib.axes.Axes
Plot axes
width : float
Cube width
origin : np.array
Origin
orientation : np.array
Orientation
"""
# Cube points
points = np.array([[-1, -1, -1],
[1, -1, -1],
[1, 1, -1],
[-1, 1, -1],
[-1, -1, 1],
[1, -1, 1],
[1, 1, 1],
[-1, 1, 1]])
points = points * (width / 2.0)
# Rotate Cube
R = euler2rot([deg2rad(i) for i in orientation], 123)
points = np.array([dot(R, pt) for pt in points])
# Translate Cube
points += origin
# Plot mesh grid
r = [-1, 1]
X, Y = np.meshgrid(r, r)
# List of sides' polygons of figure
verts = [[points[0], points[1], points[2], points[3]],
[points[4], points[5], points[6], points[7]],
[points[0], points[1], points[5], points[4]],
[points[2], points[3], points[7], points[6]],
[points[1], points[2], points[6], points[5]],
[points[4], points[7], points[3], points[0]],
[points[2], points[3], points[7], points[6]]]
# Plot sides
ax.add_collection3d(Poly3DCollection(verts,
facecolors="black",
linewidths=1,
edgecolors="red",
alpha=0.25))
def test_calculate_residuals(self):
# Camera states
# -- Camera state 0
p_G_C0 = np.array([0.0, 0.0, 0.0])
rpy_C0G = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)])
q_C0G = euler2quat(rpy_C0G)
C_C0G = C(q_C0G)
# -- Camera state 1
p_G_C1 = np.array([1.0, 1.0, 0.0])
rpy_C1G = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)])
q_C1G = euler2quat(rpy_C1G)
C_C1G = C(q_C1G)
# Features
landmark = np.array([0.0, 0.0, 10.0])
kp1 = self.cam_model.project(landmark, C_C0G, p_G_C0)[0:2]
kp2 = self.cam_model.project(landmark, C_C1G, p_G_C1)[0:2]
# Setup feature track
track_id = 0
frame_id = 1
data1 = KeyPoint(kp1, 21)
data2 = KeyPoint(kp2, 21)
track = FeatureTrack(track_id, frame_id, data1, data2)
# Setup track cam states
self.msckf.augment_state()
self.msckf.min_track_length = 2
self.msckf.cam_states[0].p_G = p_G_C0.reshape((3, 1))
self.msckf.cam_states[0].q_CG = q_C0G.reshape((4, 1))
self.msckf.cam_states[1].p_G = p_G_C1.reshape((3, 1))
self.msckf.cam_states[1].q_CG = q_C1G.reshape((4, 1))
# Test
self.msckf.calculate_residuals([track])
def test_triangulate(self):
# Camera states
# -- Camera state 0
p_G_C0 = np.array([0.0, 0.0, 0.0])
rpy_C0G = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)])
q_C0G = euler2quat(rpy_C0G)
C_C0G = C(q_C0G)
# -- Camera state 1
p_G_C1 = np.array([1.0, 1.0, 0.0])
rpy_C1G = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)])
q_C1G = euler2quat(rpy_C1G)
C_C1G = C(q_C1G)
# Features
landmark = np.array([0.0, 0.0, 10.0])
kp1 = self.cam_model.project(landmark, C_C0G, p_G_C0)[0:2]
kp2 = self.cam_model.project(landmark, C_C1G, p_G_C1)[0:2]
# Calculate rotation and translation of first and last camera states
# -- Obtain rotation and translation from camera 0 to camera 1
C_C0C1 = dot(C_C0G, C_C1G.T)
t_C0_C1C0 = dot(C_C0G, (p_G_C1 - p_G_C0))
# -- Convert from pixel coordinates to image coordinates
pt1 = self.cam_model.pixel2image(kp1)
pt2 = self.cam_model.pixel2image(kp2)
# Triangulate
p_C0_C1C0, r = self.estimator.triangulate(pt1, pt2, C_C0C1, t_C0_C1C0)
# Assert
self.assertTrue(np.allclose(p_C0_C1C0.ravel(), landmark))
def focal_length(image_width, image_height, fov):
"""Calculate focal length in the x and y axis from:
- image width
- image height
- field of view
Parameters
----------
image_width : int
Image width
image_height : int
Image height
fov : float
Field of view
Returns
-------
(fx, fy) : (float, float)
Focal length in x and y axis
"""
fx = (image_width / 2.0) / tan(deg2rad(fov) / 2.0)
fy = (image_height / 2.0) / tan(deg2rad(fov) / 2.0)
return (fx, fy)
def setUp(self):
# Generate random features
nb_features = 100
feature_bounds = {
"x": {
"min": -1.0,
"max": 1.0
},
"y": {
"min": -1.0,
"max": 1.0
},
"z": {
"min": 10.0,
"max": 20.0
}
}
self.features = rand3dfeatures(nb_features, feature_bounds)
# Pinhole Camera model
image_width = 640
image_height = 480
fov = 60
fx, fy = focal_length(image_width, image_height, fov)
cx, cy = (image_width / 2.0, image_height / 2.0)
K = camera_intrinsics(fx, fy, cx, cy)
self.cam = PinholeCameraModel(image_width, image_height, K)
# Rotation and translation of camera 0 and camera 1
self.R_0 = np.eye(3)
self.t_0 = np.zeros((3, 1))
self.R_1 = roty(deg2rad(10.0))
self.t_1 = np.array([1.0, 0.0, 0.0]).reshape((3, 1))
# Points as observed by camera 0 and camera 1
self.obs0 = self.project_points(self.features, self.cam, self.R_0,
self.t_0)
self.obs1 = self.project_points(self.features, self.cam, self.R_1,
self.t_1)
def latlon_offset(lat_ref, lon_ref, offset_N, offset_E):
"""Calculate new latitude and longitude coordinates with an offset in
North and East directions
Parameters
----------
lat_ref : float
Latitude of origin (decimal format)
lon_ref : float
Longitude of origin (decimal format)
offset_N : float
Offset in North direction (meters)
offset_E : float
Offset in East direction (meters)
Returns
-------
(lat, lon)
"""
lat_new = lat_ref + (offset_E / EARTH_RADIUS_M)
lon_new = lon_ref + (offset_N / EARTH_RADIUS_M) / cos(deg2rad(lat_ref))
return (lat_new, lon_new)
def test_residualize_track(self):
# Camera states
# -- Camera state 0
p_G_C0 = np.array([0.0, 0.0, 0.0])
rpy_C0G = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)])
q_C0G = euler2quat(rpy_C0G)
C_C0G = C(q_C0G)
# -- Camera state 1
p_G_C1 = np.array([1.0, 1.0, 0.0])
rpy_C1G = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)])
q_C1G = euler2quat(rpy_C1G)
C_C1G = C(q_C1G)
# Features
landmark = np.array([0.0, 0.0, 10.0])
kp1 = self.cam_model.project(landmark, C_C0G, p_G_C0)[0:2]
kp2 = self.cam_model.project(landmark, C_C1G, p_G_C1)[0:2]
# Setup feature track
track_id = 0
frame_id = 1
data1 = KeyPoint(kp1, 21)
data2 = KeyPoint(kp2, 21)
track = FeatureTrack(track_id, frame_id, data1, data2)
# Setup track cam states
self.msckf.augment_state()
self.msckf.min_track_length = 2
self.msckf.cam_states[0].p_G = p_G_C0.reshape((3, 1))
self.msckf.cam_states[0].q_CG = q_C0G.reshape((4, 1))
self.msckf.cam_states[1].p_G = p_G_C1.reshape((3, 1))
self.msckf.cam_states[1].q_CG = q_C1G.reshape((4, 1))
print(self.msckf.cam_states[0])
print(self.msckf.cam_states[1])
# Test
# self.msckf.enable_ns_trick = False
H_o_j, r_o_j, R_o_j = self.msckf.residualize_track(track)
plt.matshow(H_o_j)
plt.show()
def test_estimate(self):
estimator = DatasetFeatureEstimator()
# Pinhole Camera model
image_width = 640
image_height = 480
fov = 60
fx, fy = focal_length(image_width, image_height, fov)
cx, cy = (image_width / 2.0, image_height / 2.0)
K = camera_intrinsics(fx, fy, cx, cy)
cam_model = PinholeCameraModel(image_width, image_height, K)
# Camera states
track_cam_states = []
# -- Camera state 0
p_G_C0 = np.array([0.0, 0.0, 0.0])
rpy_C0G = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)])
q_C0G = euler2quat(rpy_C0G)
C_C0G = C(q_C0G)
track_cam_states.append(CameraState(0, q_C0G, p_G_C0))
# -- Camera state 1
p_G_C1 = np.array([1.0, 0.0, 0.0])
rpy_C1G = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)])
q_C1G = euler2quat(rpy_C1G)
C_C1G = C(q_C1G)
track_cam_states.append(CameraState(1, q_C1G, p_G_C1))
# Feature track
p_G_f = np.array([[0.0], [0.0], [10.0]])
kp0 = KeyPoint(cam_model.project(p_G_f, C_C0G, p_G_C0)[0:2], 0)
kp1 = KeyPoint(cam_model.project(p_G_f, C_C1G, p_G_C1)[0:2], 0)
track = FeatureTrack(0, 1, kp0, kp1, ground_truth=p_G_f)
estimate = estimator.estimate(cam_model, track, track_cam_states)
self.assertTrue(np.allclose(p_G_f.ravel(), estimate.ravel(), atol=0.1))
def update(self, motor_inputs, dt):
"""Update quadrotor motion model
Parameters
----------
motor_inputs : np.array
Motor inputs
dt : float
Time difference
"""
# States
ph = self.states[0]
th = self.states[1]
ps = self.states[2]
p = self.states[3]
q = self.states[4]
r = self.states[5]
x = self.states[6] # NOQA
y = self.states[7] # NOQA
z = self.states[8] # NOQA
vx = self.states[9]
vy = self.states[10]
vz = self.states[11]
# Convert motor inputs to angular p, q, r and total thrust
A = np.array([[1.0, 1.0, 1.0, 1.0],
[0.0, -self.arm_length, 0.0, self.arm_length],
[-self.arm_length, 0.0, self.arm_length, 0.0],
[-self.d, self.d, -self.d, self.d]])
tau = dot(A, motor_inputs)
tauf = tau[0]
taup = tau[1]
tauq = tau[2]
taur = tau[3]
# Update
self.states[0] = ph + (p + q * sin(ph) * tan(th) + r * cos(ph) * tan(th)) * dt # NOQA
self.states[1] = th + (q * cos(ph) - r * sin(ph)) * dt # NOQA
self.states[2] = ps + ((1 / cos(th)) * (q * sin(ph) + r * cos(ph))) * dt # NOQA
self.states[3] = p + (-((self.Iz - self.Iy) / self.Ix) * q * r - (self.kr * p / self.Ix) + (1 / self.Ix) * taup) * dt # NOQA
self.states[4] = q + (-((self.Ix - self.Iz) / self.Iy) * p * r - (self.kr * q / self.Iy) + (1 / self.Iy) * tauq) * dt # NOQA
self.states[5] = r + (-((self.Iy - self.Ix) / self.Iz) * p * q - (self.kr * r / self.Iz) + (1 / self.Iz) * taur) * dt # NOQA
self.states[6] = x + vx * dt
self.states[7] = y + vy * dt
self.states[8] = z + vz * dt
ax = ((-self.kt * vx / self.m) + (1 / self.m) * (cos(ph) * sin(th) * cos(ps) + sin(ph) * sin(ps)) * tauf) # NOQA
ay = ((-self.kt * vy / self.m) + (1 / self.m) * (cos(ph) * sin(th) * sin(ps) - sin(ph) * cos(ps)) * tauf) # NOQA
az = (-(self.kt * vz / self.m) + (1 / self.m) * (cos(ph) * cos(th)) * tauf - self.g) # NOQA
self.states[9] = vx + ax * dt
self.states[10] = vy + ay * dt
self.states[11] = vz + az * dt
# Constrain yaw to be [-180, 180]
self.states[2] = rad2deg(self.states[2])
self.states[2] = deg2rad(self.states[2])
data=np.dot(x.data(), self.data()))
# Input is rotation matrix
elif x.shape == (3, 3):
R = x
T_x = np.block([[R, np.zeros((3, 1))], [0.0, 0.0, 0.0, 1.0]])
return np.dot(T, T_x)[0:3, 0:3]
# Input is vector (not in homogeneous coordinates)
elif len(x) != 4 and len(x) == 3:
x = x.ravel()
x = np.array([[x[0]], [x[1]], [x[2]], [1.0]])
return np.dot(T, x)[0:3]
# Input is vector (in homogeneous coordinates)
elif x.shape == (4, 1):
return np.dot(T, x)
# Input is matrix
elif x.shape == (4, 4):
return np.dot(T, x)
# Useful default transforms
R_camera_global = np.dot(rotx(deg2rad(-90.0)), rotz(deg2rad(-90.0)))
R_global_camera = np.dot(rotz(deg2rad(90.0)), rotx(deg2rad(90.0)))
T_camera_global = Transform("camera", "global", R=R_camera_global)
T_global_camera = Transform("global",
"camera",
data=np.linalg.inv(T_camera_global.data()))
def plot(self, ax):
""" Plot quadrotor
Parameters
----------
ax :
"""
# Quadrotor center
center = np.array([0.0, 0.0, 0.0])
# Quadrotor axis
front = np.array(
[center[0] + (self.arm_length * 1.0), center[1], center[2]])
left = np.array(
[center[0], center[1] + (self.arm_length * 1.0), center[2]])
up = np.array(
[center[0], center[1], center[2] + (self.arm_length * 1.0)])
# Quadrotor arms ("+" configuration)
arm_l = np.array([center[0], center[1] + self.arm_length, center[2]])
arm_r = np.array([center[0], center[1] - self.arm_length, center[2]])
arm_f = np.array([center[0] + self.arm_length, center[1], center[2]])
arm_b = np.array([center[0] - self.arm_length, center[1], center[2]])
# Quadrotor arms ("x" configuration)
if self.arm_config == "x":
R_z = rotz(deg2rad(45.0))
arm_l = np.dot(R_z, arm_l)
arm_r = np.dot(R_z, arm_r)
arm_f = np.dot(R_z, arm_f)
arm_b = np.dot(R_z, arm_b)
# Transform quadrotor
R = euler2rot(self.pose[3:6], 123)
t = np.array(self.pose[0:3])
arm_l = np.dot(R, arm_l) + t
arm_r = np.dot(R, arm_r) + t
arm_f = np.dot(R, arm_f) + t
arm_b = np.dot(R, arm_b) + t
front = np.dot(R, front) + t
left = np.dot(R, left) + t
up = np.dot(R, up) + t
center = np.dot(R, center) + t
# Plot quadrotor
ax.scatter(*center)
ax.plot([center[0], front[0]], [center[1], front[1]],
[center[2], front[2]],
color="r")
ax.plot([center[0], left[0]], [center[1], left[1]],
[center[2], left[2]],
color="g")
ax.plot([center[0], up[0]], [center[1], up[1]], [center[2], up[2]],
color="b")
for arm in [arm_l, arm_r, arm_f, arm_b]:
ax.plot([center[0], arm[0]], [center[1], arm[1]],
[center[2], arm[2]],
color="black",
marker="o")
def update(self, setpoints, actual, yaw, dt):
"""Update controller
Parameters
----------
setpoints : np.array - 1x3
Position setpoints in world frame (x, y, z)
actual : np.array - 1x3
Actual position in world frame (x, y, z)
yaw : float
Yaw setpoint
dt : float
Time difference
Returns
-------
outputs : np.array - 1x4
Position controller output where the elements represent roll, pitch,
yaw and throttle
"""
# Check rate
self.dt += dt
if self.dt < 0.01:
return self.outputs
# Update RPY errors relative to quadrotor by incorporating yaw
errors = [0.0, 0.0, 0.0]
errors[0] = setpoints[0] - actual[0]
errors[1] = setpoints[1] - actual[1]
errors[2] = setpoints[2] - actual[2]
euler = [0.0, 0.0, actual[3]]
R = euler2rot(euler, 123)
errors = dot(R, errors)
# Roll, pitch, yaw and thrust
r = -1.0 * self.y_controller.update(errors[1], 0.0, dt)
p = self.x_controller.update(errors[0], 0.0, dt)
y = yaw
t = 0.5 + self.z_controller.update(errors[2], 0.0, dt)
outputs = [r, p, y, t]
# Limit roll, pitch
for i in range(2):
if outputs[i] > deg2rad(30.0):
outputs[i] = deg2rad(30.0)
elif outputs[i] < deg2rad(-30.0):
outputs[i] = deg2rad(-30.0)
# Limit thrust
if outputs[3] > 1.0:
outputs[3] = 1.0
elif outputs[3] < 0.0:
outputs[3] = 0.0
# Yaw first if threshold reached
if fabs(yaw - actual[3]) > deg2rad(2.0):
outputs[0] = 0.0
outputs[1] = 0.0
# Keep track of outputs
self.outputs = outputs
self.dt = 0.0
return outputs
def update(self, setpoints, actual, dt):
"""Update
Parameters
----------
setpoints : np.array - 1x3
Attitude setpoints in body frame (roll, pitch, yaw, z)
actual : np.array - 1x3
Actual attitude in body frame (roll, pitch, yaw, z)
yaw : float
Yaw setpoint
dt : float
Time difference
Returns
-------
outputs : np.array - 1x4
Attitude controller output where the elements represent roll, pitch,
yaw and throttle
"""
self.dt += dt
if self.dt < 0.001:
return self.outputs
# update yaw error
actual_yaw = rad2deg(actual[2])
setpoint_yaw = rad2deg(setpoints[2])
error_yaw = setpoint_yaw - actual_yaw
if error_yaw > 180.0:
error_yaw -= 360.0
elif error_yaw < -180.0:
error_yaw += 360.0
error_yaw = deg2rad(error_yaw)
# roll pitch yaw
r = self.roll_controller.update(setpoints[0], actual[0], self.dt)
p = self.pitch_controller.update(setpoints[1], actual[1], self.dt)
y = self.yaw_controller.update(error_yaw, 0.0, self.dt)
# thrust
max_thrust = 5.0
t = max_thrust * setpoints[3] # convert relative to true thrust
t = max_thrust if t > max_thrust else t # limit thrust
t = 0.0 if t < 0 else t # limit thrust
# map roll, pitch, yaw and thrust to motor outputs
outputs = [0.0, 0.0, 0.0, 0.0]
outputs[0] = -p - y + t
outputs[1] = -r + y + t
outputs[2] = p - y + t
outputs[3] = r + y + t
# limit outputs
for i in range(4):
if outputs[i] > max_thrust:
outputs[i] = max_thrust
elif outputs[i] < 0.0:
outputs[i] = 0.0
# keep track of outputs
self.outputs = outputs
self.dt = 0.0
return outputs
def test_world_to_body(self):
wp = np.array([1.0, 1.0, 3.0])
pos = np.array([0.0, 0.0, 4.0])
yaw = deg2rad(45.0)
R = euler2rot(np.array([0.0, 0.0, yaw]), 321)
def test_calc_yaw_to_waypoint(self):
wp = np.array([1.0, 1.0, 3.0])
pos = np.array([0.0, 0.0, 3.0])
heading = self.wp_controller.calc_yaw_to_waypoint(wp, pos)
self.assertEqual(heading, deg2rad(45.0))
def update(self, pos, yaw, dt):
"""Update waypoint controller
Parameters
----------
pos : numpy array
robot position
yaw : float
Yaw setpoint
dt : float
Time difference
Returns
-------
carrot_pt : numpy array
carrot point
"""
# Time pre-check
self.dt += dt
if self.dt < 0.01:
return self.outputs
# Get new waypoint
wp = self.update_waypoint(pos)
# Calculate yaw error
# actual_yaw = yaw
# setpoint_yaw = self.calc_yaw_to_waypoint(wp, pos)
# error_yaw = setpoint_yaw - actual_yaw
# if error_yaw > pi:
# error_yaw -= 2 * pi
# elif error_yaw < -pi:
# error_yaw += 2 * pi
# Calculate along and cross track errors
# R_BG = euler2rot(np.array([0.0, 0.0, yaw]), 321).T
# error_p_B = np.dot(R_BG, (wp - pos))
# error_at = error_p_B[1]
# error_ct = error_p_B[0]
# error_z = error_p_B[2]
error_at = wp[0] - pos[0]
error_ct = wp[1] - pos[1]
error_z = wp[2] - pos[2]
# Roll, pitch, yaw and thrust
r = -1 * self.ct_ctrl.update(error_ct, 0.0, self.dt)
p = self.at_ctrl.update(error_at, 0.0, self.dt)
# r = 0.0
# p = 0.0
# y = self.yaw_ctrl.update(error_yaw, 0.0, self.dt)
y = 0.0
t = 0.5 + self.z_ctrl.update(error_z, 0.0, self.dt)
outputs = [r, p, y, t]
# Limit roll, pitch
for i in range(2):
if outputs[i] > deg2rad(30.0):
outputs[i] = deg2rad(30.0)
elif outputs[i] < deg2rad(-30.0):
outputs[i] = deg2rad(-30.0)
# Limit thrust
if outputs[3] > 1.0:
outputs[3] = 1.0
elif outputs[3] < 0.0:
outputs[3] = 0.0
# # Yaw first if threshold reached
# if fabs(yaw - actual[3]) > deg2rad(2.0):
# outputs[0] = 0.0
# outputs[1] = 0.0
# Keep track of outputs
self.outputs = outputs
self.dt = 0.0
return outputs
def test_plot(self):
# Gimbal initial guess
# # -- tau (x, y, z, roll, pitch, yaw)
# tau_s = np.array([-0.045, -0.085, 0.08, 0.0, 0.0, 0.0])
# tau_d = np.array([0.01, 0.0, -0.03, pi / 2.0, 0.0, -pi / 2.0])
# # -- link (theta, alpha, a, d)
# alpha = pi / 2.0
# offset = -pi / 2.0
# roll = 0.0
# pitch = 0.1
# link1 = np.array([offset + roll, alpha, 0.0, 0.045])
# link2 = np.array([pitch, 0.0, 0.0, 0.0])
# # Optimized gimbal params
# offset = -pi / 2.0
# roll = 0.0
# pitch = 0.0
#
# tau_s = np.array([-0.0419, -0.0908, 0.0843, 0.0189, -0.0317, -0.0368])
# tau_d = np.array([0.0032, 0.0007, -0.0302, 1.5800, 0.0123, -1.5695])
# link1 = np.array([offset + roll, 1.5731, -0.0012, 0.0404])
# link2 = np.array([pitch, 0.0, 0.0, 0.0])
# Real 2 initial guess
# offset = -pi / 2.0
# roll = 0.0
# pitch = 0.0
# tau_s = np.array([0.0, -0.08, 0.1143, 0.0, 0.0, 0.0])
# tau_d = np.array([0.0, 0.0, 0.0, pi / 2.0, 0.0, -pi / 2.0])
# link1 = np.array([offset + roll, # theta
# pi / 2.0, # alpha
# 0.0, # a
# 0.0]) # d
# link2 = np.array([pitch, 0.0, 0.0, 0.0])
# Real 2 optimized params
offset = -pi / 2.0
roll = 0.0
pitch = 0.0
tau_s = np.array([
0.01519, -0.036509, 0.096354,
deg2rad(2.15041521),
deg2rad(1.41060977),
deg2rad(18.52822371)
])
link1 = np.array([
offset + roll, # theta
deg2rad(83.57483), # alpha
-0.0050, # a
0.01683
]) # d
link2 = np.array([pitch, 0.0, 0.0, 0.0])
tau_d = np.array([
-0.00029, 0.00138, -0.03656,
deg2rad(84.14281),
deg2rad(0.28527),
deg2rad(-91.17418)
])
# -- Gimbal model
gimbal_model = GimbalModel(tau_s=tau_s,
tau_d=tau_d,
link1=link1,
link2=link2)
# T_ds = gimbal_model.T_ds()
# p_s = np.array([1.0, 0.0, 0.0, 1.0])
# Plot gimbal model
plot = PlotGimbal(gimbal=gimbal_model,
show_static_frame=True,
show_base_frame=True,
show_end_frame=True,
show_dynamic_frame=True)
plot.set_attitude([0.0, 0.0])
plot.plot()
# Plot
debug = True
# debug = False
if debug:
plt.show()
def test_deg2rad(self):
r = deg2rad(360)
self.assertTrue((r - 2 * math.pi) < 0.0001)
