def test_kinorrt_cases(stability_solver, max_samples=100):
viewer = Viewer()
_itbl.loadOpenGL()
keyword = 'wall'
neighbor_r = 10
#object_shape = [0.5, 0.5, 0.2, 0.2]
# x_init = (4.5, 0.5, 0)
# x_goal = (-3, 7.5, np.pi)
object_shape = [0.5, 0.2, 0.2, 0.2]
x_init = (4.5, 0.2, 0)
x_goal = (-3, 7.2, 0)
X_dimensions = np.array([(-8, 8), (0, 7 + object_shape[1] * 4),
(-np.pi, np.pi)])
world_key = 'vert'
dist_weight = [1, 0.5, 0.1]
dist_cost = 0.2
manipulator = point_manipulator()
step_length = 10
mnp_fn_max = 100
goal_kch = [1, 1, 0]
app = iTM2d(object_shape, example=keyword)
viewer.set_renderer(app)
viewer.init()
X = SearchSpace(X_dimensions)
the_object = part(app.target, object_shape)
rrt_tree = RRTManipulationStability(X, x_init, x_goal,
environment(app.collision_manager),
the_object, manipulator, max_samples,
neighbor_r, world_key)
rrt_tree.mnp_fn_max = mnp_fn_max
rrt_tree.dist_weight = dist_weight
rrt_tree.cost_weight[0] = dist_cost
rrt_tree.step_length = step_length
rrt_tree.goal_kch = goal_kch
rrt_tree.initialize_stability_margin_solver(stability_solver)
t_start = time.time()
paths = rrt_tree.search()
stability_solver.save_hvfc_params_path(paths)
t_end = time.time()
print('time:', t_end - t_start)
app.get_path((paths[0], paths[2]))
app.get_nodes(rrt_tree.trees[0].nodes)
app.get_tree(rrt_tree)
viewer.start()
return t_end - t_start
def test_kinorrt_cases(keyword, max_samples,max_time):
viewer = Viewer()
_itbl.loadOpenGL()
manipulator = point_manipulator()
mnp_fn_max = None
step_length = 2
neighbor_r = 5
dist_cost = 1
if keyword == 'sofa':
neighbor_r = 5
object_shape = [1.75, 1, 1.5, 0.75]
X_dimensions = np.array([(0, 14.5), (0, 14.5), (-np.pi, np.pi)]) # dimensions of Search Space
x_init = (0, OBJECT_SHAPE[0] / 2, np.pi / 2) # starting location
x_goal = (OBJECT_SHAPE[0] * 5 + BLOCK_W, HALLWAY_W + BLOCK_H * 2 - OBJECT_SHAPE[1] - 1, np.pi / 2)
world_key = 'planar'
dist_weight = 50
dist_cost = 0.5
manipulator = doublepoint_manipulator()
elif keyword == 'corner':
neighbor_r = 5
object_shape = [0.5, 0.5, 0.2, 0.2]
X_dimensions = np.array([(0, 4), (0, 4), (-np.pi, np.pi)])
x_init = (3, 0.5, 0) # starting location
x_goal = (0.707, 0.707, -np.pi / 4)
# x_init = (2, 0.5, 0)
# x_goal = (-3, 0.5, -np.pi / 2)
world_key = 'vert'
dist_weight = 1
manipulator = doublepoint_manipulator()
mnp_fn_max = 6
step_length = 2
goal_kch = [1, 1, 1]
elif keyword == 'wall':
neighbor_r = 10
object_shape = [0.5,0.5,0.2,0.2]
X_dimensions = np.array([(-8, 8), (0, 7 + object_shape[1]*4), (-np.pi, np.pi)])
x_init = (4.5, 0.5, 0)
x_goal = (-3, 7.5, 0)
world_key = 'vert'
dist_weight = 1
dist_cost = 0.2
manipulator = point_manipulator()
step_length = 10
mnp_fn_max = 50
goal_kch = [1,1,0]
elif keyword == 'table':
object_shape = [1, 1, 0.2, 0.2]
X_dimensions = np.array([(0, 4), (0, 4), (-2*np.pi, 2*np.pi)])
# x_init = (3, 0.5, 0) # starting location
# x_goal = (0.707, 0.707, np.pi / 4)
x_init = (2, 1, 0)
x_goal = (2, 1, -np.pi/2)
world_key = 'vert'
dist_weight = 1
manipulator = doublepoint_manipulator()
mnp_fn_max = 6
step_length = 3.14
goal_kch = [0.1, 0.1, 10]
elif keyword == 'obstacle_course':
object_shape = [0.5, 0.5, 0.2, 0.2]
X_dimensions = np.array([(-2.5,3), (0, 4), (-2*np.pi, 2*np.pi)])
x_init = (-2.5, 1.5, 0)
x_goal = (2.5, 1.5, 0)
world_key = 'vert'
dist_weight = 0.08
dist_cost = 1
manipulator = doublepoint_manipulator()
mnp_fn_max = 6.15
goal_kch = [0.7,0.2,0]
elif keyword == 'peg-in-hole-v':
object_shape = [0.45, 1, 0.2, 0.2]
X_dimensions = np.array([(-2, 1), (-2, 3), (-np.pi, np.pi)])
x_init = (-2,1,0)
x_goal = (0,-1,0)
world_key = 'vert'
dist_weight = 1
manipulator = doublepoint_manipulator()
mnp_fn_max = 50
goal_kch = [0.5, 0.2, 0.8]
init_mnp = [Contact((-0.45, 0.8), (1, 0), 0), Contact((0.45, 0.8), (-1, 0), 0)]
elif keyword == 'peg-in-hole-p':
object_shape = [0.45, 1, 0.2, 0.2]
X_dimensions = np.array([(-2, 3), (0,2.5), (-np.pi, np.pi)])
x_init = (3,2.5,0)
x_goal = (-1,0.5,np.pi/2)
world_key = 'planar'
dist_weight = 1
manipulator = doublepoint_manipulator()
mnp_fn_max = 50
goal_kch = [1, 1, 1]
elif keyword == 'unpacking':
object_shape = [0.39, 1, 0.2, 0.2]
X_dimensions = np.array([(-2, 2), (-0.5, 2.5), (-np.pi, np.pi)])
x_init = (-0.5,0,0)
x_goal = (1,1.39,-np.pi/2)
world_key = 'vert'
dist_weight = 1
manipulator = doublepoint_manipulator()
mnp_fn_max = 100
goal_kch = [1, 1, 1]
elif keyword == 'book':
object_shape = [1, 0.2, 0.2, 0.2]
X_dimensions = np.array([(-4.5, 4.5), (2, 3.5), (-2*np.pi, 2*np.pi)])
x_init = (0,2.2,0)
x_goal = (-2,3,-np.pi/2)
world_key = 'vert'
dist_weight = 1
manipulator = doublepoint_manipulator()
mnp_fn_max = 15
goal_kch = [0.01, 0.1, 1]
allow_contact_edges = [True, False, True, False]
elif keyword == 'pushing':
object_shape = [0.5, 1, 0.2, 0.2]
X_dimensions = np.array([(-3.1, 3.1), (-2.6,4), (-np.pi, np.pi)])
x_init = (-2,-1.25,0)
x_goal = (2.1,2.75,0)
world_key = 'planar'
dist_weight = 1
manipulator = point_manipulator()
mnp_fn_max = 50
goal_kch = [1, 1, 1]
else:
print('Wrong case keyword!')
raise
app = iTM2d(object_shape, example=keyword)
viewer.set_renderer(app)
viewer.init()
X = SearchSpace(X_dimensions)
if keyword == 'book':
the_object = part(app.target, object_shape, allow_contact_edges)
else:
the_object = part(app.target, object_shape)
rrt_tree = RRT1(X, x_init, x_goal, environment(app.collision_manager), the_object, manipulator,
max_samples, neighbor_r, world_key)
rrt_tree.mnp_fn_max = mnp_fn_max
rrt_tree.dist_weight = dist_weight
rrt_tree.cost_weight[0] = dist_cost
rrt_tree.step_length = step_length
rrt_tree.goal_kch = goal_kch
rrt_tree.max_time = max_time
t_start = time.time()
if keyword == 'peg-in-hole-v':
paths, ifsuccess, n_samples = rrt_tree.search(init_mnp)
else:
paths, ifsuccess, n_samples = rrt_tree.search()
t_end = time.time()
print('time:', t_end - t_start)
app.get_path(paths)
app.get_nodes(rrt_tree.trees[0].nodes)
app.get_tree(rrt_tree)
viewer.start()
return t_end - t_start, len(paths[2]), n_samples, ifsuccess
(0, 100)]) # dimensions of Search Space
# obstacles
Obstacles = np.array([(20, 20, 20, 40, 40, 40), (20, 20, 60, 40, 40, 80),
(20, 60, 20, 40, 80, 40), (60, 60, 20, 80, 80, 40),
(60, 20, 20, 80, 40, 40), (60, 20, 60, 80, 40, 80),
(20, 60, 60, 40, 80, 80), (60, 60, 60, 80, 80, 80)])
x_init = (0, 0, 0) # starting location
x_goal = (100, 100, 100) # goal location
Q = np.array([(8, 4)]) # length of tree edges
r = 1 # length of smallest edge to check for intersection with obstacles
max_samples = 1024 # max number of samples to take before timing out
prc = 0.1 # probability of checking for a connection to goal
# create Search Space
X = SearchSpace(X_dimensions, Obstacles)
# create rrt_search
rrt = RRT(X, Q, x_init, x_goal, max_samples, r, prc)
path = rrt.rrt_search()
# plot
plot = Plot("rrt_3d")
plot.plot_tree(X, rrt.trees)
if path is not None:
plot.plot_path(X, path)
plot.plot_obstacles(X, Obstacles)
plot.plot_start(X, x_init)
plot.plot_goal(X, x_goal)
plot.draw(auto_open=True)
def test_kinorrt_cases(stability_solver, max_samples=100):
viewer = Viewer()
_itbl.loadOpenGL()
step_length = 2
neighbor_r = 5
dist_cost = 10
object_shapes = [[1, 0.5], [0.5, 0.5]]
X_dimensions = np.array([(-1.5, 1.5), (-1.5, 2),
(-1.5 * np.pi, 1.5 * np.pi)])
x_init = (0, 0, 0)
x_goal = (0, 0, np.pi)
world_key = 'vert'
dist_weight = 1
manipulator = doublepoint_manipulator(
np.array([[-1.5, -1.5, 0., -1.5], [-0., 1.5, 1.5, 1.5]]))
mnp_fn_max = 100
goal_kch = [0.1, 0.1, 1]
app = iTM2d(object_shapes)
viewer.set_renderer(app)
viewer.init()
X = SearchSpace(X_dimensions)
the_object = in_hand_part(app.targets, object_shapes)
app.target_T(the_object.T0, the_object.T1)
envir = in_hand_environment(app.collision_manager)
rrt_tree = RRTManipulation(X, x_init, x_goal, envir, the_object,
manipulator, max_samples, neighbor_r, world_key)
rrt_tree.env_mu = 0.8
rrt_tree.mnp_mu = 0.8
rrt_tree.mnp_fn_max = mnp_fn_max
rrt_tree.dist_weight = dist_weight
rrt_tree.cost_weight[0] = dist_cost
rrt_tree.step_length = step_length
rrt_tree.goal_kch = goal_kch
rrt_tree.initialize_stability_margin_solver(stability_solver)
t_start = time.time()
init_mnp = [
Contact((-0.5, 0.25), (1, 0), 0),
Contact((0.5, 0.25), (-1, 0), 0)
]
# rrt_tree.x_goal = (0,0,np.pi/2)
# path, mnp_path = rrt_tree.search(init_mnp)
rrt_tree.x_goal = (0, 0, np.pi)
paths = rrt_tree.search(init_mnp)
t_end = time.time()
print('time:', t_end - t_start)
whole_path = []
envs = []
mnps = []
modes = []
for q in paths[2][2:]:
ps = rrt_tree.trees[0].edges[q].path
ps.reverse()
m = np.array(rrt_tree.trees[0].edges[q].mode)
current_envs = []
current_modes = []
current_path = []
mnp = rrt_tree.trees[0].edges[q].manip
for p in ps:
_, env = rrt_tree.check_collision(p)
if len(mnp) + len(env) != len(m):
if len(mnp) + len(env) == sum(m != CONTACT_MODE.LIFT_OFF):
m = m[m != CONTACT_MODE.LIFT_OFF]
else:
print('env contact error')
continue
current_modes.append(m)
current_path.append(p)
current_envs.append(env)
current_mnps = [mnp] * len(current_path)
whole_path += current_path
envs += current_envs
modes += current_modes
mnps += current_mnps
print(whole_path, envs, modes, mnps)
# app.get_path(paths[0],mnps)
results = traj_optim_static((whole_path, envs, modes, mnps), rrt_tree)
app.get_path(np.array(results).reshape(-1, 3), mnps)
app.get_nodes(rrt_tree.trees[0].nodes)
app.get_tree(rrt_tree)
viewer.start()
return
def test_kinorrt_cases(stability_solver, max_samples=100):
viewer = Viewer()
_itbl.loadOpenGL()
step_length = 2
neighbor_r = 5
dist_cost = 10
object_shapes = [[1, 0.5], [0.5, 0.5]]
X_dimensions = np.array([(-1.5, 1.5), (-1.5, 2),
(-1.5 * np.pi, 1.5 * np.pi)])
x_init = (0, 0, 0)
x_goal = (0, 0, np.pi)
world_key = 'vert'
dist_weight = 1
manipulator = doublepoint_manipulator(
np.array([[-1.5, -1.5, 0., -1.5], [-0., 1.5, 1.5, 1.5]]))
mnp_fn_max = 100
goal_kch = [0.1, 0.1, 1]
app = iTM2d(object_shapes)
viewer.set_renderer(app)
viewer.init()
X = SearchSpace(X_dimensions)
the_object = in_hand_part(app.targets, object_shapes)
app.target_T(the_object.T0, the_object.T1)
envir = in_hand_environment(app.collision_manager)
rrt_tree = RRTManipulation(X, x_init, x_goal, envir, the_object,
manipulator, max_samples, neighbor_r, world_key)
rrt_tree.env_mu = 0.8
rrt_tree.mnp_mu = 0.8
rrt_tree.mnp_fn_max = mnp_fn_max
rrt_tree.dist_weight = dist_weight
rrt_tree.cost_weight[0] = dist_cost
rrt_tree.step_length = step_length
rrt_tree.goal_kch = goal_kch
rrt_tree.initialize_stability_margin_solver(stability_solver)
t_start = time.time()
init_mnp = [
Contact((-0.5, 0.25), (1, 0), 0),
Contact((0.5, 0.25), (-1, 0), 0)
]
# rrt_tree.x_goal = (0,0,np.pi/2)
# path, mnp_path = rrt_tree.search(init_mnp)
rrt_tree.x_goal = (0, 0, np.pi)
path, mnp_path = rrt_tree.search(init_mnp)
t_end = time.time()
print('time:', t_end - t_start)
app.get_path(path, mnp_path)
app.get_nodes(rrt_tree.trees[0].nodes)
app.get_tree(rrt_tree)
viewer.start()
return
elif keyword == 'pushing':
object_shape = [0.5, 1, 0.2, 0.2]
X_dimensions = np.array([(-3.1, 3.1), (-2.6,4), (-np.pi, np.pi)])
x_init = (-2,-1.25,0)
x_goal = (2.1,2.75,0)
world_key = 'planar'
dist_weight = 1
manipulator = point_manipulator()
mnp_fn_max = 50
goal_kch = [1, 1, 1]
app = iTM2d(object_shape, example=keyword)
viewer.set_renderer(app)
viewer.init()
X = SearchSpace(X_dimensions)
if keyword == 'book':
the_object = part(app.target, object_shape, allow_contact_edges)
else:
the_object = part(app.target, object_shape)
rrt_tree = RRTManipulation(X, x_init, x_goal, environment(app.collision_manager), the_object, manipulator,
max_samples, neighbor_r, world_key)
rrt_tree.mnp_fn_max = mnp_fn_max
rrt_tree.dist_weight = dist_weight
rrt_tree.cost_weight[0] = dist_cost
rrt_tree.step_length = step_length
rrt_tree.goal_kch = goal_kch
rrt_tree.initialize_stability_margin_solver(stability_solver)