Let's now use A$*$ to plan a path for a simple planar robot arm with $2$ rotational joints to catch a moving target with known trajectory, while avoiding static obstacles in the workspace.

The skeleton code is provided. Your job is to implement the following function:def your$\_$planner$($workspace$,$ obstacles, robot$\_$init, target, speed$\_$limit$)$The function takes the workspace, the polygonal obstacles, the robot initial configuration, the target trajectory and the joint speed limit as input arguments, and returns a path of the robot to catch the target. The planner should try to catch the target as quick as possible. Forward kinematics, goal checking and some other helper functions are provided.

You planner needs the following components:

Discretization of the configuration space $($the graph$)$

A$*$ search over the graph to find the path

Collision checking between the robot and the obstacles $($the pruning function during the search$)$

Your planner should be complete and optimal with respect to the grid discretization, and be able to handle any initial configuration, any target trajectory and any polygonal obstacles.

You should also do your best to make your planner run as fast as you can, since in practice, the objects on the conveyer belt move fast and your planner only has limited amount of time to plan its motion.

The skeleton code:

import numpy as np

import matplotlib.pyplot as plt

# catch a moving target

def fkRR$($a$)$:

$"""$

$"""$

out $=$ np$.$eye$(4)$

out$[0,3]=$ np$.$cos$($a$[0])*1+$ np$.$cos$($a$[0]+$a$[1])*1$

out$[1,3]=$ np$.$sin$($a$[0])*1+$ np$.$sin$($a$[0]+$a$[1])*1$

out$[0,0]=$ np$.$cos$($a$[0]+$a$[1])$

out$[0,1]=-$np$.$sin$($a$[0]+$a$[1])$

out$[1,0]=$ np$.$sin$($a$[0]+$a$[1])$

out$[1,1]=$ np$.$cos$($a$[0]+$a$[1])$

return out

def plotArm$($theta$)$:

$"""$

$"""$

T$\_$sd $=$ fkRR$($theta$)$

plt$.$plot$($T$\_$sd$[0,3],$T$\_$sd$[1,3],"$ro$")$ plt$.$plot$([$T$\_$sd$[0,3],$T$\_$sd$[0,3]+0\mathrm{.}2*$T$\_$sd$[0,0]],[$T$\_$sd$[1,3],$T$\_$sd$[1,3]+0\mathrm{.}2*$T$\_$sd$[1,0]],"$r$")$

a$1=$ theta$[0]$

a$2=$ theta$[1]$ plt$.$plot$([0,$np$.$cos$($a$1)],[0,$np$.$sin$($a$1)],"$k$")$

# plt$.$plot$($np$.$cos$($a$1),$np$.$sin$($a$1),"$bo$")$

plt$.$plot$([$np$.$cos$($a$1),$np$.$cos$($a$1)+$np$.$cos$($a$1+$a$2)],[$np$.$sin$($a$1),$np$.$sin$($a$1)+$np$.$sin$($a$1+$a$2)],"$k$")$ plt$.$plot$($np$.$cos$($a$1)+$np$.$cos$($a$1+$a$2),$np$.$sin$($a$1)+$np$.$sin$($a$1+$a$2),"$bo$")$

# plt$.$axis$("$equal$")$

return

def plotObst$($obstacles$)$:

$"""$

$"""$

for o in obstacles:

plt$.$plot$($o$[$:$,0],$ o$[$:$,1],"$k$")$

plt$.$plot$([$o$[-1,0],$ o$[0,0]],[$o$[-1,1],$o$[0,1]],"$k$")$

return

def your$\_$planner$($workspace$,$ obstacles, robot$\_$init, target, speed$\_$limit$)$:

$"""$

$"""$

# TODO #

comment out the following lines and replace it with your algorithm

example$\_$sol $=[]$

for i in range$(50)$:

example$\_$sol.append$($robot$\_$init $+$ np$.$array$([-$i$*$speed$\_$limit$[0],2*$i$*$speed$\_$limit$[1]]))$

return example$\_$sol, len$($example$\_$sol$)-1$

def ifReachGoal$($theta$,$ target$\_$xy$)$:

$"""$

$"""$

T $=$ fkRR$($theta$)$

error $=$ abs$($T$[0,3]-$target$\_$xy$[0])+$ abs$($T$[1,3]-$ target$\_$xy$[1])$

print$("$error:$",$ error$)$

if error $<0\mathrm{.}1$:

return True

return False

def example$\_$test$()$:

$"""$

$"""$

# environment and obstacles

workspace $=$ np$.$array$([[-5,5],[-5,5]])$

obstacles $=[]$

obstacles.append$($ np$.$array$([[0\mathrm{.}5,0\mathrm{.}5],[1,0\mathrm{.}7],[1,1\mathrm{.}1],[0\mathrm{.}5,1]]))$ obstacles.append$($ np$.$array$([[-0\mathrm{.}7,-0\mathrm{.}5],[-2\mathrm{.}1,-0\mathrm{.}5],[-2,-1\mathrm{.}7],[-0\mathrm{.}6,-1\mathrm{.}8]]))$

# initial position of the robot and joint speed limits

robot$\_$init $=$ np$.$array$([-0\mathrm{.}3,-0\mathrm{.}4])$

speed$\_$limit $=$ np$.$array$([0\mathrm{.}05,0\mathrm{.}05])$

# moving targets

target $=[]$

for j in range$(80)$:

target.append$([$np$.$sin$(0\mathrm{.}1*$j$-0\mathrm{.}3),$np$.$cos$(0\mathrm{.}1*$j$-0\mathrm{.}3)])$

target $=$ np$.$array$($target$)$

sol$\_$path, sol$\_$cost $=$ your$\_$planner$($workspace$,$ obstacles, robot$\_$init, target, speed$\_$limit$)$ print$($sol$\_$path$)$

fig $=$ plt$.$figure$($figsize$=(5,5))$

for i in range$($len$($target$))$:

plt$.$clf$()$

# PlotGrids$($grid$)$

plt$.$xlim$([-3,3])$

plt$.$ylim$([-3,3])$

plt$.$xticks$($np$.$arange$(-3,3,1))$

plt$.$yticks$($np$.$arange$(-3,3,1))$

plt$.$plot$($target$[$:$,0],$target$[$:$,1],"$r:$")$

plt$.$plot$($target$[$i$,0],$target$[$i$,1],"$ro$")$

plotObst$($obstacles$)$

if i $<$ len$($sol$\_$path$)$:

plotArm$($sol$\_$path$[$i$])$

plt$.$draw$()$ plt$.$pause$(0\mathrm{.}1)$

if i $<$ len$($sol$\_$path$)$ and ifReachGoal$($sol$\_$path$[$i$],$ target$[$i$])$:

print$("$The robot catch the target!"$)$

break

return

if $\_\_$name$\_\_=="\_\_$main$\_\_"$:

# Two test instances are provided. You can design more instances by yourself. example$\_$test$()$

# example$\_$test$2()$