1e-9 * np.eye(sys_opts['nx']
), # :nparray: (nx x nx) initial state uncertainty
'max_dims': np.array(
[2e-1, 2e-1]
), # :nparray: (2,) maximum axis length of ellipse in each dimension
# 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()
'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
'D': 0.10*np.eye(sys_opts['nw']), # :nparray: (nw x nw) ellipse describing uncertainty
'E0': 0.10*np.eye(sys_opts['nx']), # :nparray: (nx x nx) initial state uncertainty
'max_dims': np.array([5, 5]), # :nparray: (2,) maximum axis length of ellipse in each dimension
# currently only 2D supported
'Q': 10*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
sim1 = RRTSimulator(tree=rrt_tree,
opts=options)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')