# currently only 2D supported
'Q': np.diag((1, 1, 0.5, 0.5)), # :nparray: (nx x nx) TVLQR Q
'R': np.eye(sys_opts['nu']), # :nparray: (nu x nu) TVLQR R
}
# run rrt
print('\nRRT Expanding...')
rrt_tree.treeExpansion(options)
print('\nPlotting...')
rrt_final_path = rrt_tree.finalPath()
drawScene(scene, size=(15, 15))
plt.xlabel('Theta1 (Radians)', fontsize=20)
plt.ylabel('Theta2 (Radians)', fontsize=20)
plt.title('Note: Positions are modulo 2pi', fontsize=16)
plt.suptitle('Furuta RRT', fontsize=25, y=0.925)
drawTree(rrt_tree, color='blue')
drawPath(rrt_final_path, color='red')
print('Finished\n')
plt.draw()
plt.pause(0.001) # hack to show plots realtime
# run rerrt
print('RERRT Expanding...')
rerrt_tree.treeExpansion(options)
print('\nPlotting...')
rerrt_final_path = rerrt_tree.finalPath()
drawScene(scene, size=(15, 15))
plt.xlabel('Theta1 (Radians)', fontsize=20)
plt.ylabel('Theta2 (Radians)', fontsize=20)
plt.title('Note: Positions are modulo 2pi', fontsize=16)
plt.suptitle('Furuta RERRT', fontsize=25, y=0.925)
'direction':
'backward', # :'backward'/'forward': determine tree growth direction
'track_children':
True, # :bool: keep record of children of node
'extend_by':
10, # :int: num timesteps to simulate in steer function with each extension
'goal_sample_rate':
0.20, # :float: goal sample freq. (out of 1)
'sample_dim':
2, # :int: Determine how many dimensions to sample in, e.g. 2 for 2D
}
print('\nTree Expanding...')
tree.treeExpansion(run_options)
print('\nPlotting...')
final_path = tree.finalPath()
#for n in final_path:
# print(f'num children: {len(n.children)}')
# print([(child.x, child.u) for child in n.children])
#print(f'max children: {max((len(n.children) for n in final_path))}')
# order determines what gets occluded in figure
drawScene(scene, size=(15, 15))
plt.xlabel('X (position)', fontsize=20)
plt.ylabel('Y (position)', fontsize=20)
plt.title('Car RRT', fontsize=25)
# drawScene is called first as it creates the figure then shows region+obstacles
drawTree(tree, color='blue')
drawPath(final_path, color='red')
print('Finished\n')
plt.show()