def drive_to_target(ugv, target, fault):
pid = PID(0)
ugv.theta = math.radians(0)
i = 0
distance = 100
start_point = np.array([ugv.x, ugv.y, ugv.theta])
max_iter = 270
steering_curr = 0.0
velocity = 0.4
steering_next = 0.0
velocity_next = 0.0
y = np.empty((max_iter, 2))
y_dot = np.empty((max_iter, 2))
while (i != max_iter):
ugv.path_data.append([ugv.x, ugv.y, ugv.theta, i])
ugv.calculateError(target)
pid.calculatePID(ugv.distance_error, ugv.heading_error, 0.1)
# print("GAMMA: %s" % pid.steering)
steering = pid.steering
velocity_next = pid.velocity
y[i, :] = [ugv.y + velocity * math.sin(ugv.theta), i]
ugv.dynamics(velocity, 0.0, fault, 0.1)
y_dot[i, :] = [ugv.y, i]
i += 1
path = np.array(ugv.path_data)
print(path.shape)
plt.plot(path[:, 0], path[:, 1])
plt.xlabel("x (meters)")
plt.ylabel("y (meters)")
plt.title("UGV Path: Problem 1.b)")
# plt.gcf().gca().add_artist(Wedge(center=(int(start_point[0]), int(start_point[1])), r=1, theta1=45, theta2=75, width=1))
plt.scatter(start_point[0], start_point[1], marker='o', color='blue')
plt.show()
# print(path)
# print(y_dot)
noise_data = np.subtract(y_dot[:, 0], y[:, 0])
plt.plot(path[:, 0], noise_data)
plt.show()
class Controller():
def __init__(self, initial_position, initial_orientation, odometry_topic,
velocity_topic, k):
self.x = initial_position[0]
self.y = initial_position[1]
self.z = initial_position[2]
self.roll = initial_orientation[0]
self.pitch = initial_orientation[1]
self.yaw = initial_orientation[2]
self.odom = odometry_topic
self.cmd_vel = velocity_topic
self.distance = 0.0
self.heading = 0.0
self.pid = PID(k)
self.last_time = 0.0
self.threshold = 0.1
def odomCallback(self, data):
self.x = float(round(data.pose.pose.position.x, 1))
self.y = float(round(data.pose.pose.position.y, 1))
self.z = float(round(data.pose.pose.position.z, 1))
orientation_quaternion = data.pose.pose.orientation
orientation_list = [
orientation_quaternion.x, orientation_quaternion.y,
orientation_quaternion.z, orientation_quaternion.w
]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
self.roll = float(roll)
self.pitch = float(pitch)
self.yaw = float(yaw)
####
# Parameters: target = [x*, y*]
# Returns: Euclidean Distance Error, Heading Error
####
def calculateError(self, target):
delta_x = np.clip(target[0] - self.x, -1e50, 1e50)
delta_y = np.clip(target[1] - self.y, -1e50, 1e50)
desired_heading = math.atan2(delta_y, delta_x)
self.heading = desired_heading - self.yaw
delta_x2 = delta_x**2
delta_y2 = delta_y**2
if math.isinf(delta_x2):
delta_x2 = 1e25
if math.isinf(delta_y2):
delta_y2 = 1e25
self.distance = math.sqrt(delta_x2 + delta_y2)
def moveToTarget(self):
rospy.init_node('mouseToJoy', anonymous=True)
#### Setup Odometry Subscriber
rospy.Subscriber(self.odom,
Odometry,
self.odomCallback,
queue_size=1,
tcp_nodelay=True)
#### Setup Velocity Publisher
velocityPublisher = rospy.Publisher(self.cmd_vel,
Twist,
queue_size=1,
tcp_nodelay=True)
rate = rospy.Rate(20) # 20hz
data = Twist()
x_target = float(input("Enter X Target: \n"))
y_target = float(input("Enter Y Target: \n"))
target = np.array([x_target, y_target])
while not rospy.is_shutdown():
self.calculateError(target)
dt = rospy.Time.now().to_sec() - self.last_time
self.pid.calculatePID(self.distance, self.heading, dt)
self.last_time = rospy.Time.now().to_sec()
print(self.x, self.y)
if math.fabs(self.distance) > self.threshold:
data.linear.x = 0.2
#data.linear.x = self.pid.velocity
# msg.angular.z = 100
data.angular.z = self.pid.steering
else:
data.linear.x = 0.0
data.angular.z = 0.0
print("Target Reached")
subprocess.call(
"rosservice call /zed/zed_node/set_pose 0 0 0 0 0 0 ")
velocityPublisher.publish(data)
rate.sleep()
sys.exit(1)
velocityPublisher.publish(data)
rate.sleep()
class Bicycle():
def __init__(self, phi=0.0):
self.x = 0.0
self.y = 0.0
self.theta = math.radians(0)
self.drift_angle = phi
self.desired_x = 0
self.desired_y = 0
self.heading_error = 0.0
self.distance_error = 100000.0
self.distance = 0.1
self.T = 50.0
self.max_rad = 38.0
self.max_vel = 1.0
self.max_iter = 1200 * 8
self.L = 0.19
self.pid = PID()
self.path_data = []
def dynamics(self, v, gamma, dt):
self.x = np.clip(self.x, -1e5, 1e5)
self.x = np.float64(self.x)
if self.drift_angle == 0.0:
# ideal dynamics
yaw_dot = ((v / self.L) * (math.tan(gamma))) * dt
x_dot = (v * math.cos(self.theta)) * dt
y_dot = (v * math.sin(self.theta)) * dt
else:
# fault dynamics
noise_gamma = gamma + self.drift_angle + np.random.normal(
0, 0.0125)
yaw_dot = ((v / self.L) * (math.tan(noise_gamma))) * dt
x_dot = (v * math.cos(self.theta)) * dt
y_dot = (v * math.sin(self.theta)) * dt
# yaw_dot = np.clip(yaw_dot, -math.radians(45), math.radians(45))
# x_dot = np.clip(x_dot, -0.1, 0.1)
# y_dot = np.clip(y_dot, -0.1, 0.1)
self.theta = self.theta + yaw_dot
self.x = self.x + x_dot
self.y = self.y + y_dot
def angdiff(self, a, b):
diff = a - b
if diff < 0.0:
diff = (diff % (-2 * math.pi))
if diff < (-math.pi):
diff = diff + 2 * math.pi
else:
diff = (diff % 2 * math.pi)
if diff > math.pi:
diff = diff - 2 * math.pi
return diff
def calculate_error(self):
delta_x = np.clip(self.desired_x - self.x, -1e50, 1e50)
delta_y = np.clip(self.desired_y - self.y, -1e50, 1e50)
desired_heading = math.atan2(delta_y, delta_x)
self.heading_error = self.angdiff(desired_heading, self.theta)
delta_x2 = delta_x**2
delta_y2 = delta_y**2
if math.isinf(delta_x2):
delta_x2 = 1e25
if math.isinf(delta_y2):
delta_y2 = 1e25
self.distance_error = math.sqrt(delta_x2 + delta_y2) - self.distance
def drive_open_loop(self):
self.path_data = []
self.drift_angle = 0
i = 0
while (i != self.max_iter):
self.path_data.append([self.x, self.y])
dt = (1.0 / self.T)
self.dynamics(0.36, -0.00615835, dt)
i += 1
path = np.asarray(self.path_data)
#print(path.shape)
plt.scatter(path[:, 0], path[:, 1])
plt.xlabel("x (meters)")
plt.ylabel("y (meters)")
plt.title("UGV Path: Problem 1.a)")
plt.show()
def drive_along_path(self):
# the target angle of the circular path being followed
self.x = 4.0
self.y = 0.0
self.theta = math.radians(90)
alpha = math.radians(0)
cx = 2
cy = 2
r = 2
i = 0
self.desired_x = cx + r * math.cos(alpha)
self.desired_y = cy + r * math.sin(alpha)
while (i != self.max_iter):
# Update trajectory
if self.distance_error < self.distance * 4:
alpha += math.radians(2)
self.desired_x = cx + r * math.cos(alpha)
self.desired_y = cy + r * math.sin(alpha)
# if i % 10 == 0:
# print("Desired X: ", self.desired_x)
# print("Desired Y: ", self.desired_y)
# print("Alpha: ", alpha)
# Compute error
self.calculate_error()
# print("Heading error: ", self.heading_error)
# print("Distance error: ", self.distance_error)
# Compute controls
dt = (1.0 / self.T)
self.pid.calculatePID(self.distance_error, self.heading_error, dt)
# Execute controls
self.dynamics(self.pid.velocity, self.pid.steering, dt)
# Save new pose data and increment coutner
i += 1
self.path_data.append([self.x, self.y, self.theta])
# Control scheme executed, save path data
self.path_data = np.array(self.path_data)
class Rover:
def __init__(self):
self.x = 0
self.y = 0
self.test_x = 0
self.test_y = 0
self.test_theta = 0
self.theta = 0
self.desired_x = 0
self.desired_y = 0
self.desired_theta = 0
self.k = [0.1, 0.0, 0.0, 1, 0.0, 0.0]
self.max_rad = 38.0
self.max_vel = 1.0
self.L = 0.19
self.pid = PID(self.k)
self.noise_path = []
self.test_path = []
self.prev_noise = 0.0
self.noise_functions = np.array(
[[2.00000000e+01, 6.62201978e-03, -1.99941291e+01],
[-5.10091508, -0.06503655, 5.08946158],
[-0.01754703, 1.0, 0.06324038]])
def dynamics(self, v, gamma, fault):
#noise dynamics
theta_dot = ((v / self.L) * (math.tan(gamma)))
x_dot = v * math.cos(self.theta)
y_dot = v * math.sin(self.theta)
self.x = np.clip(self.x, -1e5, 1e5)
self.x = np.float128(self.x)
noise = self.noise_functions[fault, 0] * np.exp(
self.noise_functions[fault, 1] *
self.x) + self.noise_functions[fault, 2]
#derivatives
self.theta = self.theta + np.clip(theta_dot,
-1 * math.radians(self.max_rad),
math.radians(self.max_rad))
self.x = self.x + np.clip(x_dot, -1 * self.max_vel, self.max_vel)
self.y = self.y + np.clip(y_dot, -1 * self.max_vel,
self.max_vel) + (noise - self.prev_noise)
self.prev_noise = noise
#test dynamics
theta_dot = ((v / self.L) * (math.tan(gamma)))
x_dot = v * math.cos(self.test_theta)
y_dot = v * math.sin(self.test_theta)
self.x = np.clip(self.test_x, -1e5, 1e5)
self.x = np.float128(self.test_x)
#derivatives
self.test_theta = self.test_theta + np.clip(
theta_dot, -1 * math.radians(self.max_rad),
math.radians(self.max_rad))
self.test_x = self.test_x + np.clip(x_dot, -1 * self.max_vel,
self.max_vel)
self.test_y = self.test_y + np.clip(y_dot, -1 * self.max_vel,
self.max_vel)
def driveAlongPath(self, fault):
self.x = 0
self.y = 0
self.test_x = 0
self.test_y = 0
self.theta = math.radians(45)
self.test_theta = math.radians(45)
distance = 1000000.0
prev_noise = 0.0
self.desired_x = 10.0
self.desired_y = 10.0
delta_x = self.desired_x - self.x
delta_y = self.desired_y - self.y
while distance >= 0.5:
test_delta_x = np.clip(self.test_desired_x - self.test_x, -1e50,
1e50)
test_delta_y = np.clip(self.test_desired_y - self.test_y, -1e50,
1e50)
self.test_desired_theta = math.atan2(test_delta_x, test_delta_y)
test_delta_theta = self.test_desired_theta - self.test_theta
test_delta_x2 = test_delta_x**2
test_delta_y2 = test_delta_y**2
if math.isinf(test_delta_x2):
test_delta_x2 = 1e25
if math.isinf(test_delta_y2):
test_delta_y2 = 1e25
test_distance = math.sqrt(test_delta_x2 + test_delta_y2)
delta_x = np.clip(self.desired_x - self.x, -1e50, 1e50)
delta_y = np.clip(self.desired_y - self.y, -1e50, 1e50)
self.desired_theta = math.atan2(delta_y, delta_x)
delta_theta = self.desired_theta - self.test_theta
delta_x2 = delta_x**2
delta_y2 = delta_y**2
if math.isinf(delta_x2):
delta_x2 = 1e25
if math.isinf(delta_y2):
delta_y2 = 1e25
distance = math.sqrt(delta_x2 + delta_y2)
self.noise_path.append([self.x, self.y])
self.test_path.append([self.test_x, self.test_y])
self.pid.calculatePID(distance, delta_theta)
self.dynamics(self.pid.v, self.pid.g, fault)
self.noise_path = np.asarray(self.noise_path)
self.test_path = np.asarray(self.test_path)
plt.plot(self.noise_path[:, 0],
self.noise_path[:, 1],
color='red',
linewidth=2)
plt.plot(self.test_path[:, 0],
self.test_path[:, 1],
color='blue',
linewidth=2)
plt.xlabel('x')
plt.ylabel('y')
plt.title('UGV Path')
plt.show()
