# rrt.show()
# plt.show()
import arm
qstart = np.array([90, 10, 0, -150, 0, 0, 0]) * math.pi / 180
qgoal = np.array([20, -15, 0, 0, 0, 10, 0]) * math.pi / 180
pointcloud = {'points': np.array([[0, 0, 0]]), 'means': 0, 'sigmas': 0}
arm_data_dict = arm.arm_map_create(pointcloud, qstart, qgoal)
arm_random_sampler = partial(arm.arm_random_sample, eps=0.1)
arm_config = {'collision_check': arm.arm_collision_check,
'random_sample': arm_random_sampler,
'steer': arm.arm_steer,
'dist': arm.arm_dist_func,
'goal_region': arm.arm_goal_region,
'feat': arm.arm_feat_single,
'num_feat': 1}
rrt = RRTConnectEnv(arm_config, arm_data_dict)
policy = DefaultPolicy()
obs = rrt.reset()
done = False
pointcloud_nums = [1, 3]
pointcloud_list = []
start_list = []
goal_list = []
for num in pointcloud_nums:
file = 'pointclouddata/processed_' + str(num) + '.mat'
points = scipy.io.loadmat(file)['save_struct'][0, 0]
pointcloud_list.append(points)
start_list.append(start)
goal_list.append(goals[num])
data_dict_list = []
config_list = []
for i in range(len(pointcloud_list)):
data_dict = arm.arm_map_create(pointcloud_list[i], start_list[i],
goal_list[i])
arm_random_sampler = partial(arm.arm_random_sample, eps=0.1)
config = {
'collision_check': arm.arm_collision_check,
'random_sample': arm_random_sampler,
'steer': arm.arm_steer,
'dist': arm.arm_dist_func,
'goal_region': arm.arm_goal_region,
'feat': feat_func,
'num_feat': num_feats
}
data_dict_list.append(data_dict)
config_list.append(config)
else:
raise Exception('Not a valid Environment')