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