Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
import numpy as np

class TrajectoryGenerator():
def __init__(self, start_pos, des_pos, T, start_vel=[0,0,0], des_vel=[0,0,0], start_acc=[0,0,0], des_acc=[0,0,0]):
def __init__(self, start_pos, des_pos, T, start_vel=(0,0,0), des_vel=(0,0,0), start_acc=(0,0,0), des_acc=(0,0,0)):
self.start_x = start_pos[0]
self.start_y = start_pos[1]
self.start_z = start_pos[2]
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -244,16 +244,18 @@ def update_points(self):

self.end_effector = np.array(self.points[self.n_links]).T

def plot(self, obstacles=[]): # pragma: no cover
def plot(self, obstacles=None): # pragma: no cover
plt.cla()
if obstacles is None:
obstacles = []

for obstacle in obstacles:
circle = plt.Circle(
(obstacle[0], obstacle[1]), radius=0.5 * obstacle[2], fc='k')
plt.gca().add_patch(circle)

for i in range(self.n_links + 1):
if i is not self.n_links:
if i != self.n_links:
plt.plot([self.points[i][0], self.points[i + 1][0]],
[self.points[i][1], self.points[i + 1][1]], 'r-')
plt.plot(self.points[i][0], self.points[i][1], 'k.')
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -275,16 +275,18 @@ def update_points(self):

self.end_effector = np.array(self.points[self.n_links]).T

def plot_arm(self, myplt, obstacles=[]): # pragma: no cover
def plot_arm(self, myplt, obstacles=None): # pragma: no cover
myplt.cla()
if obstacles is None:
obstacles = []

for obstacle in obstacles:
circle = myplt.Circle(
(obstacle[0], obstacle[1]), radius=0.5 * obstacle[2], fc='k')
myplt.gca().add_patch(circle)

for i in range(self.n_links + 1):
if i is not self.n_links:
if i != self.n_links:
myplt.plot([self.points[i][0], self.points[i + 1][0]],
[self.points[i][1], self.points[i + 1][1]], 'r-')
myplt.plot(self.points[i][0], self.points[i][1], 'k.')
Expand Down
2 changes: 1 addition & 1 deletion ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ def plot(self): # pragma: no cover
lambda event: [exit(0) if event.key == 'escape' else None])

for i in range(self.n_links + 1):
if i is not self.n_links:
if i != self.n_links:
plt.plot([self.points[i][0], self.points[i + 1][0]],
[self.points[i][1], self.points[i + 1][1]], 'r-')
plt.plot(self.points[i][0], self.points[i][1], 'ko')
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ def main(): # pragma: no cover
errors, distance = distance_to_goal(end_effector, goal_pos)

# State machine to allow changing of goal before current goal has been reached
if state is WAIT_FOR_NEW_GOAL:
if state == WAIT_FOR_NEW_GOAL:
if distance > 0.1 and not solution_found:
joint_goal_angles, solution_found = inverse_kinematics(
link_lengths, joint_angles, goal_pos)
Expand All @@ -54,7 +54,7 @@ def main(): # pragma: no cover
arm.goal = end_effector
elif solution_found:
state = MOVING_TO_GOAL
elif state is MOVING_TO_GOAL:
elif state == MOVING_TO_GOAL:
if distance > 0.1 and all(old_goal == goal_pos):
joint_angles = joint_angles + Kp * \
ang_diff(joint_goal_angles, joint_angles) * dt
Expand Down Expand Up @@ -103,7 +103,7 @@ def animation():
errors, distance = distance_to_goal(end_effector, goal_pos)

# State machine to allow changing of goal before current goal has been reached
if state is WAIT_FOR_NEW_GOAL:
if state == WAIT_FOR_NEW_GOAL:

if distance > 0.1 and not solution_found:
joint_goal_angles, solution_found = inverse_kinematics(
Expand All @@ -114,7 +114,7 @@ def animation():
arm.goal = get_random_goal()
elif solution_found:
state = MOVING_TO_GOAL
elif state is MOVING_TO_GOAL:
elif state == MOVING_TO_GOAL:
if distance > 0.1 and all(old_goal == goal_pos):
joint_angles = joint_angles + Kp * \
ang_diff(joint_goal_angles, joint_angles) * dt
Expand Down
2 changes: 1 addition & 1 deletion Mapping/grid_map_lib/grid_map_lib.py
Original file line number Diff line number Diff line change
Expand Up @@ -152,7 +152,7 @@ def set_value_from_polygon(self, pol_x, pol_y, val, inside=True):

flag = self.check_inside_polygon(x_pos, y_pos, pol_x, pol_y)

if flag is inside:
if flag == inside:
self.set_value_from_xy_index(x_ind, y_ind, val)

def calc_grid_index_from_xy_index(self, x_ind, y_ind):
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -205,7 +205,7 @@ def dijkstra_planning(sx, sy, gx, gy, road_map, sample_x, sample_y):
else:
open_set[n_id] = node

if path_found is False:
if not path_found:
return [], []

# generate final course
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,9 +35,9 @@ class Grid:
# Set in constructor
grid_size: np.ndarray
reservation_matrix: np.ndarray
obstacle_paths: list[list[Position]] = []
obstacle_paths: list[list[Position]]
# Obstacles will never occupy these points. Useful to avoid impossible scenarios
obstacle_avoid_points: list[Position] = []
obstacle_avoid_points: list[Position]

# Number of time steps in the simulation
time_limit: int
Expand All @@ -49,15 +49,18 @@ def __init__(
self,
grid_size: np.ndarray,
num_obstacles: int = 40,
obstacle_avoid_points: list[Position] = [],
obstacle_avoid_points: list[Position] | None = None,
obstacle_arrangement: ObstacleArrangement = ObstacleArrangement.RANDOM,
time_limit: int = 100,
):
if obstacle_avoid_points is None:
obstacle_avoid_points = []
self.obstacle_avoid_points = obstacle_avoid_points
self.time_limit = time_limit
self.grid_size = grid_size
self.reservation_matrix = np.zeros((grid_size[0], grid_size[1], self.time_limit))

self.obstacle_paths = []
if num_obstacles > self.grid_size[0] * self.grid_size[1]:
raise Exception("Number of obstacles is greater than grid size!")

Expand Down