Motion Planners..

import casadi import numpy as np import random import matplotlib.pyplot as plt

# Code to generate random circles in workspace circles = [] num_circles = 100 x_range = [-100, 100] y_range = x_range radius_range = [5, 10] for _ in range(num_circles): center_x = random.uniform(x_range[0], x_range[1]) center_y = random.uniform(y_range[0], y_range[1]) radius = random.uniform(radius_range[0], radius_range[1]) circles.append({ 'center': (center_x, center_y), 'radius': radius })

# Simple collision checker def coll_check(point): for circle in circles: center = circle['center'] radius = circle['radius'] if (point[0] - center[0])**2 + (point[1] - center[1])**2 <= radius**2: return False return True

# Trying path planning with optimization based approaches # Randomly sampling start and end points for _ in range(100): start_point = (random.uniform(x_range[0], x_range[1]), random.uniform(y_range[0], y_range[1])) end_point = (random.uniform(x_range[0], x_range[1]), random.uniform(y_range[0], y_range[1])) if coll_check(start_point) and coll_check(end_point): break # Visualize the circles and points plt.figure(figsize=(10, 10)) ax = plt.gca() ax.set_xlim(x_range) ax.set_ylim(y_range) ax.set_aspect('equal', adjustable='box') for circle in circles: center = circle['center'] radius = circle['radius'] circle_patch = plt.Circle(center, radius, color='blue', alpha=0.5) ax.add_patch(circle_patch) plt.scatter(start_point[0], start_point[1], color='green', s=120, marker='o') plt.scatter(end_point[0], end_point[1], color='red', s=120, marker='X') plt.title('Random Circles in Workspace') plt.grid(True) plt.show()

from ompl import base as ob from ompl import geometric as og import math # No of states N = 100 # Planner choice among PRM, RRT, RRT* planner_choice = "RRT" # default # Path cost path_cost = 0 # Time taken for solver algo_time = 0 # Space space = ob.RealVectorStateSpace(2) bounds = ob.RealVectorBounds(2) bounds.setLow(0, x_range[0]) bounds.setHigh(0, x_range[1]) bounds.setLow(1, y_range[0]) bounds.setHigh(1, y_range[1]) space.setBounds(bounds) # Collision checker def isStateValid(state): x = state[0] y = state[1] for circle in circles: cx, cy = circle["center"] r = circle["radius"] if (x - cx)**2 + (y - cy)**2 <= r**2: return False return True ss = og.SimpleSetup(space) ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) start = ob.State(space) goal = ob.State(space) start[0], start[1] = start_point goal[0], goal[1] = end_point ss.setStartAndGoalStates(start, goal) #PRM planner if planner_choice == "PRM": planner = og.PRM(ss.getSpaceInformation()) elif planner_choice == "RRT": planner = og.RRT(ss.getSpaceInformation()) elif planner_choice == "RRTstar": planner = og.RRTstar(ss.getSpaceInformation()) ss.setPlanner(planner) solved = ss.solve(5.0) if solved: print("Found solution") path = ss.getSolutionPath() path_cost = path.length() algo_time = ss.getLastPlanComputationTime() print(path_cost) print(algo_time) path.interpolate(N) states = [] for i in range(path.getStateCount()): s = path.getState(i) states.append((s[0], s[1])) print(states) else: print("No solution found")

# Visualize solution import matplotlib.pyplot as plt plt.figure(figsize=(10, 10)) ax = plt.gca() ax.set_xlim(x_range) ax.set_ylim(y_range) ax.set_aspect('equal', adjustable='box') # Plot obstacles for circle in circles: center = circle['center'] radius = circle['radius'] ax.add_patch( plt.Circle(center, radius, color='blue', alpha=0.5) ) # Plot PRM path if solved: xs = [s[0] for s in states] ys = [s[1] for s in states] plt.plot(xs, ys, 'k-', linewidth=2) # Plot start and goal plt.scatter(start_point[0], start_point[1], color='green', s=120, marker='o') plt.scatter(end_point[0], end_point[1], color='red', s=120, marker='X') plt.title(f"Path Planning using {planner_choice} with length {path_cost:.2f}, solved in {algo_time :.4f} secs") plt.grid(True) plt.show()

model = casadi.Opti() dt = 1 # time step # Path variables x = model.variable(N,2) # x and y coordinates theta = model.variable(N) # orientation # Motion variables v = model.variable(N-1) # linear velocity w = model.variable(N-1) # angular velocity # Constraints - Non linear constraints to avoid circles for i in range(N): for circle in circles: center = circle['center'] radius = circle['radius'] model.subject_to( (x[i,0]-center[0])**2 + (x[i,1]-center[1])**2 >= radius**2 ) ### Non linear constraint - will look at optimization later # Boundary conditions model.subject_to( x[0,:] == np.array(start_point).reshape(1,2) ) model.subject_to( x[-1,:] == np.array(end_point).reshape(1,2) ) model.subject_to(v[0] == 0) # start from rest model.subject_to(v[-1] == 0) # end at rest # Box constraints model.subject_to( model.bounded(x_range[0], x[:,0], x_range[1]) ) model.subject_to( model.bounded(y_range[0], x[:,1], y_range[1]) ) # Dynamics constraints for i in range(N-1): # Kinematic equations for a differential drive robot model.subject_to( x[i+1,0] == x[i,0] + v[i]*casadi.cos(theta[i])*dt) model.subject_to( x[i+1,1] == x[i,1] + v[i]*casadi.sin(theta[i])* dt) model.subject_to( theta[i+1] == theta[i] + w[i]*dt ) # Acc. constraints can be added here if needed w_dot_max = np.pi/4 # rad/s^2 v_dot_max = 10.0 # m/s^2 for i in range(N-2): model.subject_to( casadi.fabs(w[i+1] - w[i]) <= w_dot_max * dt) model.subject_to( casadi.fabs(v[i+1] - v[i]) <= v_dot_max * dt) # Box constraints on velocities v_max = 10.0 w_max = np.pi/2 model.subject_to( model.bounded(0, v, v_max) ) model.subject_to( model.bounded(-w_max, w, w_max) ) # Minimum path length objective path_length = 0 control_cost = 0 for i in range(N-1): path_length += ( (x[i+1,0]-x[i,0])**2 + (x[i+1,1]-x[i,1])**2 ) for i in range(N-1): control_cost += v[i]**2 + w[i]**2 # Warm start - from PRM solution theta_guess = np.zeros(N) for i in range(N - 1): dx = states[i+1][0] - states[i][0] dy = states[i+1][1] - states[i][1] theta_guess[i] = np.arctan2(dy, dx) theta_guess[-1] = theta_guess[-2] for i in range(N): model.set_initial( x[i,0], states[i][0] ) model.set_initial( x[i,1], states[i][1] ) model.set_initial( theta[i], theta_guess[i]) model.minimize(path_length + control_cost) # Squared to make it smooth # Timeout model.solver('ipopt') model.solve() path = model.value(x) velocities = model.value(v) angular_velocities = model.value(w)

import matplotlib.pyplot as plt # Extract optimized path px = path[:, 0] py = path[:, 1] plt.figure(figsize=(8, 8)) ax = plt.gca() ax.set_aspect('equal', adjustable='box') ax.set_xlim(x_range) ax.set_ylim(y_range) # Obstacles for circle in circles: center = circle["center"] radius = circle["radius"] ax.add_patch( plt.Circle(center, radius, color="gray", alpha=0.4) ) # Optimized path plt.plot(px, py, 'r-', linewidth=2, label='Optimized Path') # Start and goal plt.scatter(px[0], py[0], color='green', s=100, label='Start', zorder=5) plt.scatter(px[-1], py[-1], color='blue', s=100, label='Goal', zorder=5) plt.title("Final Optimized Path") plt.grid(True) plt.legend() plt.show()