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
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
def visualize_cases(keyword):
viewer = Viewer()
_itbl.loadOpenGL()
manipulator = point_manipulator()
mnp_fn_max = None
step_length = 2
neighbor_r = 5
dist_cost = 1
if 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]
elif keyword == 'inhand':
object_shape = [[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)
else:
print('Wrong case keyword!')
raise
app = iTM2d(object_shape, keyword, x_init, x_goal)
viewer.set_renderer(app)
viewer.init()
if keyword == 'inhand':
the_object = in_hand_part(app.targets, object_shape)
app.target_T(the_object.T0, the_object.T1)
viewer.start()
return
def get_path(self, paths):
self.path, self.mnp_path, self.key_path, self.key_mnp_path = paths
def get_nodes(self, nodes):
self.nodes = nodes
def get_tree(self, tree):
self.tree = tree
stability_solver = None
seed_number = 1*1000
random.seed(seed_number)
np.random.seed(seed_number)
keyword = 'book'
max_samples=200
viewer = Viewer()
_itbl.loadOpenGL()
manipulator = point_manipulator()
mnp_fn_max = None
step_length = 2
neighbor_r = 5
dist_cost = 1
if 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)
world_key = 'vert'
dist_weight = 1