Module 4: Fuzzy Logic
Boolean Logic
The table below is a truth table, describing how logic operators produce their results:
x |
y |
NOT x |
NOT y |
x AND y |
x OR y |
true |
true |
false |
false |
true |
true |
true |
false |
false |
true |
false |
true |
false |
true |
true |
false |
false |
true |
false |
false |
true |
true |
false |
false |
Imagine that a person who is 6’ 4” is considered tall
. Now imagine two people,
Mickey (6’ 4”) and Donald (6’ 2”). Determine the truth value of each of these statements:
- Mickey is
tall
and Donald is tall
.
- Mickey is
tall
or Donald is tall
.
- Mickey is not
tall
.
- Donald is not
tall
.
Fuzzy Logic
Consider using real numbers in the range 0.0 to 1.0 instead of false
and true
.
In this scheme, true
would correspond to 1.0, false
would be 0.0, and values
in between would correspond to varying levels of truth.
For example, imagine that it is true
that a person who is 6’ 4” is tall
, and false
that a person who is 5’ 8” is tall
.
- On a scale of 0.0 to 1.0, how true would it be that a person who is 6’ 2” is
tall
?
- How about someone 5’ 10”?
- How about someone 6’ 0”?
Based on those insights, give fuzzy answers to these four questions you answered earlier:
- Mickey is
tall
and Donald is tall
.
- Mickey is
tall
or Donald is tall
.
- Mickey is not
tall
.
- Donald is not
tall
.
Based on those answers, give a mathematical definition for each of the following fuzzy operators:
- and
- or
- not
Create a file called fuzzy.py
. Create Python definitions for the functions
f_and()
, f_or()
, and f_not()
that implement the above mathematical definitions.
A correct solution should pass the unit tests below.
import unittest
def f_and(v1: float, v2: float) -> float:
# Your code here
def f_or(v1: float, v2: float) -> float:
# Your code here
def f_not(value: float) -> float:
# Your code here
class FuzzyTest(unittest.TestCase):
def test_and_or_not(self):
self.assertEqual(0.75, f_and(1.0, 0.75))
self.assertEqual(1.0, f_or(1.0, 0.75))
self.assertEqual(0.0, f_not(1.0))
self.assertEqual(1.0, f_not(0.0))
if __name__ == "__main__":
unittest.main()
Fuzzification
Following the above example, we will say that a height of 6’ 4” (76”) is tall
(1.0), and a height
of 5’ 8” (68”) is not tall
(0.0). To fuzzify these values is to convert them from their
original units to fuzzy values.
Add the following function to fuzzy.py
, and implement it:
def fuzzify(value: float, start: float, end: float) -> float:
# Your code here.
Also add this unit-testing method to FuzzyTest
:
def test_fuzzify(self):
for expected, height in [(1.0, 76), (0.75, 74), (0.5, 72), (0.25, 70),
(0.0, 68), (1.0, 80), (0.0, 62)]:
self.assertEqual(expected, fuzzify(height, 68, 76))
A correct solution should pass this additional unit test.
Defuzzification
To transform a fuzzy value into a useful output, we defuzzify it. For instance, we might transform
a fuzzy value for tall into an inseam length as follows:
tall |
inseam |
1.0 |
36.0 |
0.75 |
34.5 |
0.5 |
|
0.25 |
|
0.0 |
30.0 |
- What should the inseam value be for
tall
= 0.5?
- How about
tall
= 0.25?
- Write down a mathematical formula for the inseam value given the value for
tall
, according to
the pattern in this table.
Add the defuzzify()
function below to fuzzy.py
, and implement it.
def defuzzify(value: float, zero: float, one: float) -> float:
# Your code here.
Also add this unit-testing method to FuzzyTest
:
def test_defuzzify(self):
for inseam_size, fuzzy_height in [(36, 1.0), (34.5, 0.75), (33, 0.5),
(31.5, 0.25), (30, 0.0)]:
self.assertEqual(inseam_size, defuzzify(fuzzy_height, 30, 36))
A correct solution should pass this additional unit test.
Copy the following code into a new file, ir_fuzzy_input.py
.
from typing import Any
from rclpy.node import Node
from rclpy.publisher import Publisher
from rclpy.qos import qos_profile_sensor_data
from std_msgs.msg import String
from irobot_create_msgs.msg import IrIntensityVector
import fuzzy
class FuzzyIrNode(Node):
def __init__(self, robot_name: str, ir_fuzzy_start: int, ir_fuzzy_end: int):
super().__init__(f'FuzzyIrNode_{robot_name}')
self.create_subscription(IrIntensityVector, f"{robot_name}/ir_intensity",
self.ir_callback, qos_profile_sensor_data)
self.output_topic = f'{robot_name}_ir_blocked'
self.output = self.create_publisher(String, self.output_topic, qos_profile_sensor_data)
self.debug_topic = f'{robot_name}_debug_topic'
self.debug = self.create_publisher(String, self.debug_topic, qos_profile_sensor_data)
self.ir_fuzzy_start = ir_fuzzy_start
self.ir_fuzzy_end = ir_fuzzy_end
def publish(self, publisher: Publisher, data: Any):
output = String()
output.data = f"{data}"
publisher.publish(output)
def ir_callback(self, msg: IrIntensityVector):
out_msg = {}
debug_msg = ""
for reading in msg.readings:
f = fuzzy.fuzzify(reading.value, self.ir_fuzzy_start, self.ir_fuzzy_end)
out_msg[reading.header.frame_id] = f
debug_msg += f"{reading.header.frame_id:32}{reading.value:>4} {f:>.2f} {' ' * 10}\n"
self.publish(self.output, out_msg)
self.publish(self.debug, debug_msg)
- What will a
FuzzyIrNode
object do when spun?
- Based on your experience with the IR sensors, what value would you suggest for
ir_fuzzy_start
?
- How about
ir_fuzzy_end
?
Defuzzified motor controller node
Copy the following code into a new file, ir_fuzzy_avoider.py
:
import sys, curses
from typing import Dict
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from std_msgs.msg import String
from geometry_msgs.msg import TwistStamped
from ir_fuzzy_input import FuzzyIrNode
from curses_runner import CursesNode, run_curses_nodes
import fuzzy
class FuzzyDriveNode(Node):
def __init__(self, robot_name: str, fuzzy_topic: str, x_limit: float, z_limit: float):
super().__init__(f"FuzzyDriveNode_{robot_name}")
self.x_limit = x_limit
self.z_limit = z_limit
self.motors = self.create_publisher(TwistStamped, f"{robot_name}/cmd_vel_stamped", qos_profile_sensor_data)
self.create_subscription(String, fuzzy_topic, self.fuzzy_callback, qos_profile_sensor_data)
def fuzzy_callback(self, msg: String):
fuzzy_values = eval(msg.data)
blocked = {"left": 0.0, "right": 0.0, "ir_intensity": 0.0}
for ir, value in fuzzy_values.items():
for area in blocked:
if area in ir:
blocked[area] = fuzzy.f_or(blocked[area], value)
t = self.make_twist()
t.twist.linear.x = fuzzy.defuzzify(fuzzy.f_not(blocked["ir_intensity"]), 0, self.x_limit)
turn_limit = self.z_limit * (-1.0 if blocked["left"] > blocked["right"] else 1.0)
t.twist.angular.z = fuzzy.defuzzify(fuzzy.f_or(blocked["left"], blocked["right"]), 0, turn_limit)
self.motors.publish(t)
def make_twist(self) -> TwistStamped:
t = TwistStamped()
t.header.frame_id = "base_link"
t.header.stamp = self.get_clock().now().to_msg()
return t
def main(stdscr):
cmd = parse_cmd_line_values()
rclpy.init()
sensor_node = FuzzyIrNode(sys.argv[1], cmd['min_ir'], cmd['max_ir'])
curses_node = CursesNode(sensor_node.debug_topic, 2, stdscr)
drive_node = FuzzyDriveNode(sys.argv[1], sensor_node.output_topic, cmd['x_limit'], cmd['z_limit'])
run_curses_nodes(stdscr, [drive_node, curses_node, sensor_node])
rclpy.shutdown()
def parse_cmd_line_values() -> Dict[str,float]:
parsed = {}
for arg in sys.argv:
if '=' in arg:
parts = arg.split('=')
parsed[parts[0]] = float(parts[1])
return parsed
if __name__ == '__main__':
if len(sys.argv) < 6:
print("Usage: python3 ir_fuzzy_avoider.py robot_name min_ir=value max_ir=value x_limit=value z_limit=value")
else:
curses.wrapper(main)
- What will a
FuzzyDriveNode
object do when spun?
- Based on your experience with the motors, what value would you suggest for
x_limit
?
- How about
z_limit
?
- Write a command-line invocation of
ir_fuzzy_avoider.py
that will run it with the four values you specified.
(We will test it in a moment.)
Running a fuzzy controller
Copy curses_runner.py
from module2
into your module4
folder. Then run ir_fuzzy_avoider
with the
command-line arguments you specified.
- Overall, how well does your robot perform as an obstacle avoider?
- Do you think the
ir_fuzzy_start
value enables the robot to start turning when it should?
- Do you think the
ir_fuzzy_end
value puts the robot into a sharp turn soon enough to avoid objects?
- Do you think the
x_limit
value is fast enough to enable the robot to make progress but slow enough to
give it ample time to avoid hitting objects?
- Do you think the
z_limit
value is a good match to the x_limit
value?
- Based on your observations, experiment with some different values for these four variables. For each variation
that you try, record its impact on the robot’s performance. Create at least three distinct combinations of
values. Which combination performed the best? Why?
Fuzzy navigation
Copy odometry_math.py
from module3
into module4
. Then
copy the code below into a new file called goal_fuzzy_input.py
:
from typing import Any
from rclpy.node import Node
from rclpy.publisher import Publisher
from rclpy.qos import qos_profile_sensor_data
from nav_msgs.msg import Odometry
from std_msgs.msg import String
from geometry_msgs.msg import Point
from odometry_math import find_euclidean_distance, find_roll_pitch_yaw, find_angle_diff, find_goal_heading
import fuzzy
class FuzzyGoalNode(Node):
def __init__(self, robot_name: str, goal_x: float, goal_y: float, angle_limit: float):
super().__init__(f'FuzzyGoalNode_{robot_name}')
self.create_subscription(Odometry, f"{robot_name}/odom", self.odom_callback, qos_profile_sensor_data)
self.output_topic = f'{robot_name}_goal_error'
self.output = self.create_publisher(String, self.output_topic, qos_profile_sensor_data)
self.debug_topic = f'{robot_name}_debug_topic'
self.debug = self.create_publisher(String, self.debug_topic, qos_profile_sensor_data)
self.goal = Point()
self.goal.x = goal_x
self.goal.y = goal_y
self.distance_limit = None
self.angle_limit = angle_limit
def publish(self, publisher: Publisher, data: Any):
output = String()
output.data = f"{data}"
publisher.publish(output)
def odom_callback(self, msg: Odometry):
errors = {'left': 0.0, 'right': 0.0, 'distance': 0.0}
distance_diff = find_euclidean_distance(self.goal, msg.pose.pose.position)
if self.distance_limit is None:
self.distance_limit = distance_diff
goal_direction = find_goal_heading(msg.pose.pose.position, self.goal)
r, p, yaw = find_roll_pitch_yaw(msg.pose.pose.orientation)
angle_diff = find_angle_diff(yaw, goal_direction)
if angle_diff > 0:
errors['right'] = fuzzy.fuzzify(angle_diff, 0.0, self.angle_limit)
else:
errors['left'] = fuzzy.fuzzify(-angle_diff, 0.0, self.angle_limit)
dist = fuzzy.fuzzify(distance_diff, 0.0, self.distance_limit)
errors['distance'] = dist
self.publish(self.output, errors)
debug = f"{msg.pose.pose.position}{' ' * 10}"
debug += f"\ndistance diff: {distance_diff:.2f} ({self.distance_limit}) {' ' * 10}"
debug += f"\nyaw: {yaw:.2f} goal_direction: {goal_direction:.2f}{' ' * 10}"
debug += f"\nangle diff: {angle_diff:.2f} ({self.angle_limit}){' ' * 10}"
debug += f"\ndistance: {errors['distance']:.2f}{' ' * 10}"
debug += f"\nleft: {errors['left']:.2f}{' ' * 10}"
debug += f"\nright: {errors['right']:.2f}{' ' * 10}"
self.publish(self.debug, debug)
- What will a
FuzzyGoalNode
object do when spun?
- What do you think the term error means in this context?
- Based on your experience with odometry, what might be a good value to use for
angle_limit
?
- How is
distance_limit
determined? Why do you think it is determined in this way?
Copy the code below into a new file called goal_fuzzy_navigator.py
:
import sys, curses
from typing import Dict
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from std_msgs.msg import String
from geometry_msgs.msg import TwistStamped
from irobot_create_msgs.msg import InterfaceButtons
from goal_fuzzy_input import FuzzyGoalNode
from curses_runner import CursesNode, run_curses_nodes
import fuzzy
class FuzzyDriveNode(Node):
def __init__(self, robot_name: str, fuzzy_topic: str, x_limit: float, z_limit: float):
super().__init__(f"FuzzyDriveNode_{robot_name}")
self.x_limit = x_limit
self.z_limit = z_limit
self.override_stop = False
self.motors = self.create_publisher(TwistStamped, f"{robot_name}/cmd_vel_stamped", qos_profile_sensor_data)
self.create_subscription(String, fuzzy_topic, self.fuzzy_callback, qos_profile_sensor_data)
self.create_subscription(InterfaceButtons, f"/{robot_name}/interface_buttons", self.button_callback, qos_profile_sensor_data)
def fuzzy_callback(self, msg: String):
fuzzy_values = eval(msg.data)
t = self.make_twist()
if not self.override_stop:
t.twist.linear.x = fuzzy.defuzzify(fuzzy_values["distance"], 0, self.x_limit)
turn_limit = self.z_limit * (1.0 if fuzzy_values["left"] > fuzzy_values["right"] else -1.0)
t.twist.angular.z = fuzzy.defuzzify(fuzzy.f_or(fuzzy_values["left"], fuzzy_values["right"]), 0, turn_limit)
self.motors.publish(t)
def make_twist(self) -> TwistStamped:
t = TwistStamped()
t.header.frame_id = "base_link"
t.header.stamp = self.get_clock().now().to_msg()
return t
def button_callback(self, msg: InterfaceButtons):
if msg.button_1.is_pressed or msg.button_2.is_pressed or msg.button_power.is_pressed:
self.override_stop = True
def main(stdscr):
cmd = parse_cmd_line_values()
rclpy.init()
sensor_node = FuzzyGoalNode(sys.argv[1], cmd['goal_x'], cmd['goal_y'], cmd['angle_limit'])
curses_node = CursesNode(sensor_node.debug_topic, 2, stdscr)
drive_node = FuzzyDriveNode(sys.argv[1], sensor_node.output_topic, cmd['x_limit'], cmd['z_limit'])
run_curses_nodes(stdscr, [drive_node, curses_node, sensor_node])
rclpy.shutdown()
def parse_cmd_line_values() -> Dict[str,float]:
parsed = {}
for arg in sys.argv:
if '=' in arg:
parts = arg.split('=')
parsed[parts[0]] = float(parts[1])
return parsed
if __name__ == '__main__':
if len(sys.argv) < 5:
print("Usage: python3 goal_fuzzy_navigator.py robot_name goal_x=value goal_y=value angle_limit=value x_limit=value z_limit=value")
robot_name = sys.argv[1] if len(sys.argv) > 1 else "robot_name"
print(f"Odometry reset:\nros2 service call /{robot_name}/reset_pose irobot_create_msgs/srv/ResetPose\n")
else:
curses.wrapper(main)
- What will a
FuzzyDriveNode
object do when spun?
- What similarities and differences do you see between this version of
FuzzyDriveNode
and
the version we created for use with the IR sensors?
- How does pressing an interface button cause the robot to stop moving?
- Based on your prior experience with motors, what do you think would be suitable values
for
x_limit
and z_limit
?
- Run the program with only the name of the robot as a command-line argument. It will
print a command you can use to reset the odometry. Run that command before running the program
again. The second time you run the program, set values for all of its command-line arguments.
What does the robot do when it runs?
- Try the program again, but this time use different values for
x_limit
and z_limit
.
If previously the robot was driving quickly, this time, have it drive slowly. If previously
it was driving slowly, this time, have it drive quickly. How does the change in speed
affect its performance?
Triangular fuzzification
Examine the function below, and add it to fuzzy.py
:
def triangle(value: float, start: float, peak: float, end: float) -> float:
if value <= peak:
return fuzzify(value, start, peak)
else:
return f_not(fuzzify(value, peak, end))
- What do you think this function will do?
- Why do you think it is named triangle?
Now replace the code that calculates dist
with the following:
dist = fuzzy.triangle(distance_diff, 0.0, self.distance_limit, self.distance_limit * 2)
- How do you expect the behavior of the robot to change?
- How does this illustrate the utility of the triangle concept for fuzzification?
- Run the modified program. How did the behavior of the robot change, if at all?
- Is it necessary that the triangle is symmetrical? What might be a reason to have
a non-symmetrical triangle?
- Try a non-symmetrical triangle. How does the robot’s behavior change?
One more variation
Replace the code that calculates dist
with the following:
either_turn = fuzzy.f_or(errors['left'], errors['right'])
dist = fuzzy.triangle(distance_diff, 0.0, self.distance_limit, self.distance_limit * 2)
errors['distance'] = fuzzy.f_and(dist, fuzzy.f_not(either_turn))
- How do you expect the behavior of the robot to change?
- This block of code makes use of all three fuzzy logic operators: and, or, and not.
Explain in natural language the meaning expressed by this fuzzy logic formulation.
- Run the modified program. How did the robot’s behavior change, if at all?
- Having now explored three variations of this program, which do you prefer? Why?