def create_bottom_walls(x_lim, y_lim, env):
bottom_wall = box_body(env, x_lim * 2, y_lim * 2, 0.135, name='bottom_wall', color=(.82, .70, .55))
bottom_wall_x = x_lim / 2.0
set_xy(bottom_wall, bottom_wall_x, 0)
env.Add(bottom_wall)
side_wall = box_body(env, y_lim * 2, 0.01 * 2, 0.2 * 2,
name='side_wall_1',
color=(.82, .70, .55))
place_body(env, side_wall, (-x_lim + bottom_wall_x, 0, np.pi / 2), base_name='bottom_wall')
side_wall = box_body(env, y_lim * 2, 0.01 * 2, 0.2 * 2,
name='side_wall_2',
color=(.82, .70, .55))
place_body(env, side_wall, (x_lim + bottom_wall_x, 0, np.pi / 2), base_name='bottom_wall')
side_wall = box_body(env, x_lim * 2, 0.01 * 2, 0.2 * 2,
name='side_wall_3',
color=(.82, .70, .55))
place_body(env, side_wall, (bottom_wall_x, y_lim, 0), base_name='bottom_wall')
entrance_width = 1.2
left_door_length = (x_lim - entrance_width / 2.0) - 1
right_door_length = (x_lim - entrance_width / 2.0) + 1
entrance_left = box_body(env, left_door_length, 0.01 * 2, 0.2 * 2,
name='entrance_left',
color=(.82, .70, .55))
entrance_right = box_body(env, right_door_length, 0.01 * 2, 0.2 * 2,
name='entrance_right',
color=(.82, .70, .55))
place_body(env, entrance_left, (bottom_wall_x + (-x_lim - (entrance_width / 2.0 + 1)) / 2.0, -y_lim, 0),
base_name='bottom_wall')
place_body(env, entrance_right, (bottom_wall_x + (x_lim + (entrance_width / 2.0 - 1)) / 2.0, -y_lim, 0),
base_name='bottom_wall')
def simple_push(env):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'room1.xml')
obstacle_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
table_names = ['table1', 'table2']
place_body(env,
cylinder_body(env, .07, .05, name='blue_cylinder', color=BLUE),
(1.65, -.6, PI / 4), 'table1')
object_names = [
str(body.GetName()) for body in env.GetBodies()
if not body.IsRobot() and str(body.GetName()) not in obstacle_names
]
start_constrained = {'blue_cylinder': 'table1'}
goal_constrained = {
'blue_cylinder': ('table2',
Point(
get_point(env.GetKinBody('blue_cylinder')) +
np.array([-1.65, -1., 0.])))
#'blue_cylinder': ('table1', Point(get_point(env.GetKinBody('blue_cylinder')) + np.array([-.2,.8, 0.])))
}
return ManipulationProblem(function_name(inspect.stack()),
object_names=object_names,
table_names=table_names,
start_constrained=start_constrained,
goal_constrained=goal_constrained)
def create_doors(x_lim, y_lim, door_x, door_y, door_width, th, env):
if th == 0:
middle_wall_size = (door_y - door_width / 2.0 - (-y_lim)) / 4.0
left_wall_size = (y_lim - door_width / 2.0 - door_y) / 2.0
middle_door_y = door_y - middle_wall_size - (door_width / 2.)
right_wall_size =(y_lim/2.0 - middle_door_y ) / 2.0
elif th == np.pi / 2.:
right_wall_size = (door_x - door_width / 2.0 - (-x_lim)) / 2.0
left_wall_size = (x_lim - door_width / 2.0 - door_x) / 2.0
left_wall = box_body(env, 0.04 * 2, left_wall_size * 2, 1 * 2,
name='left_wall', color=(0, 0, 0))
right_wall = box_body(env,
0.04 * 2, right_wall_size * 2, 1 * 2,
name='right_wall',
color=(0, 0, 0))
middle_wall = box_body(env,
0.04 * 2, middle_wall_size * 2, 1 * 2,
name='middle_wall',
color=(0, 0, 0))
place_body(env, left_wall, (door_x, door_y + left_wall_size + (door_width / 2.), th),
base_name='bottom_wall')
place_body(env, middle_wall, (door_x, door_y - middle_wall_size - (door_width / 2.), th),
base_name='bottom_wall')
place_body(env, right_wall, (door_x, middle_door_y-door_width-middle_wall_size-door_width, th), base_name='bottom_wall')
wall_in_room_l = box_body(env, 0.04 * 2, 1, 2, name='wall_in_room_l', color=(0, 0, 0))
wall_in_room_r = box_body(env, 0.04 * 2, 1, 2, name='wall_in_room_r', color=(0, 0, 0))
place_body(env, wall_in_room_l, (3.3,0, np.pi/2), base_name='bottom_wall')
place_body(env, wall_in_room_r, (5.5,0, np.pi/2), base_name='bottom_wall')
def create_shelf(env, obst_x, obst_width, obst_height, name_idx, stacked_obj_name, table_name):
width = 0.4
length = 0.01
height = obst_height
top_wall_width = 0.001
bottom_wall_width = 0.0001
table_pos = aabb_from_body(env.GetKinBody(table_name)).pos()
table_x = table_pos[0]
table_y = table_pos[1]
place_body(env,
box_body(env,
width, length, height,
name='right_wall_' + str(name_idx),
color=OBST_COLOR,
transparency=OBST_TRANSPARENCY),
(table_x + .0, table_y + obst_x - (obst_width - .05) / 2, 0),
stacked_obj_name)
place_body(env,
box_body(env,
width, length, height,
name='left_wall_' + str(name_idx),
color=OBST_COLOR,
transparency=OBST_TRANSPARENCY),
(table_x + .0, table_y + obst_x + (obst_width - .05) / 2, 0),
stacked_obj_name)
place_body(env,
box_body(env,
length, obst_width - 0.05, height,
name='back_wall_' + str(name_idx),
color=OBST_COLOR,
transparency=OBST_TRANSPARENCY),
(table_x + .225, table_y + obst_x, 0),
stacked_obj_name)
place_body(env,
box_body(env,
width, obst_width - 0.05, top_wall_width,
name='top_wall_' + str(name_idx),
color=OBST_COLOR,
transparency=OBST_TRANSPARENCY),
(table_x + 0, table_y + obst_x, 0),
'back_wall_' + str(name_idx))
place_body(env,
box_body(env,
width, obst_width - 0.05, bottom_wall_width,
name='bottom_wall_' + str(name_idx),
color=OBST_COLOR,
transparency=0.5),
(table_x + 0, table_y + obst_x, 0),
stacked_obj_name)
region = create_region(env, 'place_region_' + str(name_idx),
((-1.0, 1.0), (-0.85, 0.85)),
'bottom_wall_' + str(name_idx), color=np.array((0, 0, 0, .5)))
region.draw(env)
return region
def decision_wall_namo(
env, rows, cols
): # TODO - could make variant that requires replacing things as well
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'decision_2tables.xml')
camera_trans = camera_look_at(CAMERA_POINT_SCALE *
np.array([-1.5, 1.5, 3]),
look_point=(1.5, -1.5, 0))
obstacle_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
table_names = ['table1', 'table2']
for x in range(cols):
for y in range(rows):
#if y >= rows/2 and x >= cols/2: continue
if y >= rows / 2 and x >= cols - 1:
continue # NOTE - makes one side harder than the other
place_body_on_floor(
box_body(env, .1, .1, .8, name='red%s-%s' % (x, y), color=RED),
(-.25 + x * .75 / cols, -2.5 + (y + .5) * 5 / rows, 0))
place_body(env, box_body(env, .07, .07, .2, name='blue0', color=BLUE),
(1.75, +0.2, 0), 'table1')
place_body(env, box_body(env, .07, .07, .2, name='blue1', color=BLUE),
(1.75, -0.2, 0), 'table1')
object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and \
str(body.GetName()) not in obstacle_names and 'blue' in str(body.GetName())]
floor_object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and \
str(body.GetName()) not in obstacle_names and 'red' in str(body.GetName())]
goal_config = 'initial'
goal_holding = None # False | None
goal_regions = {
'blue0': 'table2',
'blue1': 'table2',
}
return ManipulationProblem(function_name(inspect.stack()),
camera_trans=camera_trans,
floor_object_names=floor_object_names,
object_names=object_names,
table_names=table_names,
goal_config=goal_config,
goal_holding=goal_holding,
goal_regions=goal_regions)
def simple_push(env):
env.Load(ENVIRONMENTS_DIR + 'room1.xml')
obstacle_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()]
table_names = ['table1', 'table2']
place_body(env, cylinder_body(env, .07, .05, name='blue_cylinder', color=BLUE), (1.65, -.6, PI/4), 'table1')
object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names]
start_constrained = {'blue_cylinder': 'table1'}
goal_constrained = {
'blue_cylinder': ('table2', Point(get_point(env.GetKinBody('blue_cylinder')) + np.array([-1.65,-1., 0.])))
#'blue_cylinder': ('table1', Point(get_point(env.GetKinBody('blue_cylinder')) + np.array([-.2,.8, 0.])))
}
return ManipulationProblem(function_name(inspect.stack()),
object_names=object_names, table_names=table_names,
start_constrained=start_constrained,
goal_constrained=goal_constrained)
def set_fixed_object_poses(env, x_lim, y_lim):
objects = [env.GetKinBody('shelf1'), env.GetKinBody('shelf2'), env.GetKinBody('computer_table'),
env.GetKinBody('table2')]
place_body(env, env.GetKinBody('shelf1'), (x_lim + x_lim / 2.0 - 0.5, y_lim - 0.2, np.pi * 3 / 2),
base_name='bottom_wall')
place_body(env, env.GetKinBody('shelf2'), (x_lim + x_lim / 2.0 - 1.5, y_lim - 0.2, np.pi * 3 / 2),
base_name='bottom_wall')
place_body(env, env.GetKinBody('table2'), (x_lim + x_lim / 2.0 - 0.5, y_lim - 2, np.pi * 3 / 2),
base_name='bottom_wall')
#place_body(env, env.GetKinBody('computer_chair'), (4.2, -1.5, 0), base_name='bottom_wall')
obj_poses = {obj.GetName(): get_pose(obj) for obj in objects}
return obj_poses
def create_doors(x_lim, y_lim, door_x, door_y, door_width, th, env):
if th == 0:
right_wall_size = (door_y - door_width / 2.0 - (-y_lim)) / 2.0
left_wall_size = (y_lim - door_width / 2.0 - door_y) / 2.0
elif th == np.pi / 2.:
right_wall_size = (door_x - door_width / 2.0 - (-x_lim)) / 2.0
left_wall_size = (x_lim - door_width / 2.0 - door_x) / 2.0
left_wall = box_body(env,
0.04 * 2,
left_wall_size * 2,
1 * 2,
name='left_wall',
color=(.82, .70, .55))
right_wall = box_body(env,
0.04 * 2,
right_wall_size * 2,
1 * 2,
name='right_wall',
color=(.82, .70, .55))
if th == 0:
place_body(env,
left_wall,
(door_x, door_y + left_wall_size + (door_width / 2.), th),
base_name='bottom_wall')
place_body(env,
right_wall,
(door_x, door_y - right_wall_size - (door_width / 2.), th),
base_name='bottom_wall')
else:
place_body(env,
left_wall,
(door_x + left_wall_size + (door_width / 2.), door_y, th),
base_name='bottom_wall')
place_body(env,
right_wall,
(door_x - right_wall_size - (door_width / 2.), door_y, th),
base_name='bottom_wall')
set_body_transparency(left_wall, 0.3)
set_body_transparency(right_wall, 0.3)
def push_wall(env):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'room1.xml')
camera_trans = camera_look_at(CAMERA_POINT_SCALE * np.array([-1, 1, 3]),
look_point=(3, 0, 0))
obstacle_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
table_names = ['table1', 'table2']
place_body(
env, cylinder_body(env, .07, .05, name='green_cylinder', color=GREEN),
(1.65, -.5, PI / 4), 'table1')
#place_body(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN), (1.65, -.6, 0), 'table1')
#set_point(env.GetKinBody('green_box'), get_point(env.GetKinBody('green_box')) + .01*unit_z())
place_body(env, box_body(env, .05, .05, .15, name='red_box1', color=RED),
(1.50, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box2', color=RED),
(1.6, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box3', color=RED),
(1.7, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box4', color=RED),
(1.8, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box5', color=RED),
(1.9, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box6', color=RED),
(2.0, 0, 0), 'table1')
object_names = [
str(body.GetName()) for body in env.GetBodies()
if not body.IsRobot() and str(body.GetName()) not in obstacle_names
]
start_constrained = {'green_cylinder': 'table1'}
goal_constrained = {
'green_cylinder': ('table1',
Point(
get_point(env.GetKinBody('green_cylinder')) +
np.array([-.2, 1.0, 0.])))
}
return ManipulationProblem(function_name(inspect.stack()),
camera_trans=camera_trans,
object_names=object_names,
table_names=table_names,
start_constrained=start_constrained,
goal_constrained=goal_constrained)
def push_wall(env):
env.Load(ENVIRONMENTS_DIR + 'room1.xml')
camera_trans = camera_look_at(CAMERA_POINT_SCALE*np.array([-1, 1, 3]), look_point=(3, 0, 0))
obstacle_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()]
table_names = ['table1', 'table2']
place_body(env, cylinder_body(env, .07, .05, name='green_cylinder', color=GREEN), (1.65, -.5, PI/4), 'table1')
#place_body(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN), (1.65, -.6, 0), 'table1')
#set_point(env.GetKinBody('green_box'), get_point(env.GetKinBody('green_box')) + .01*unit_z())
place_body(env, box_body(env, .05, .05, .15, name='red_box1', color=RED), (1.50, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box2', color=RED), (1.6, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box3', color=RED), (1.7, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box4', color=RED), (1.8, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box5', color=RED), (1.9, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box6', color=RED), (2.0, 0, 0), 'table1')
object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names]
start_constrained = {'green_cylinder': 'table1'}
goal_constrained = {
'green_cylinder': ('table1', Point(get_point(env.GetKinBody('green_cylinder')) + np.array([-.2, 1.0, 0.])))
}
return ManipulationProblem(function_name(inspect.stack()), camera_trans=camera_trans,
object_names=object_names, table_names=table_names,
start_constrained=start_constrained,
goal_constrained=goal_constrained)
def __init__(self, env, problem_idx, problem_config=None):
self.env = env
fdir = os.path.dirname(os.path.abspath(__file__))
self.env.Load(fdir + '/resources/mover_env.xml')
self.robot = self.env.GetRobots()[0]
set_xy(self.env.GetKinBody('kitchen'), -0.3, 1.3)
# bottom wall creation
x_extents = 4
y_extents = 4
create_bottom_walls(x_extents, y_extents, self.env)
# door creation
# self.env.Remove(self.env.GetKinBody('left_wall')); self.env.Remove(self.env.GetKinBody('right_wall'))
door_width = 1. # generate_rand(1, 1.5)
door_x = (-x_extents + 1.5 + x_extents - 1.5) / 2.0 - x_extents * 0.3 + 4
door_y = (-y_extents + 1.5 + y_extents - 1.5) / 2.0 + 2
door_th = 0
create_doors(x_extents, y_extents, door_x, door_y, door_width, door_th, self.env)
set_config(self.robot, FOLDED_LEFT_ARM, self.robot.GetManipulator('leftarm').GetArmIndices())
set_config(self.robot, mirror_arm_config(FOLDED_LEFT_ARM),
self.robot.GetManipulator('rightarm').GetArmIndices())
fixed_obj_poses = set_fixed_object_poses(self.env, x_extents, y_extents)
shelf_table = env.GetKinBody('table2')
set_obj_xytheta([-1.5, -3, np.pi/2], shelf_table)
shelf_shapes, shelf_xs = generate_shelf_shapes()
shelf_regions = create_shelves(self.env, shelf_shapes, shelf_xs, 'table2')
obj_shapes = generate_shelf_obj_shapes()
shelf_objects = create_shelf_objs(self.env, obj_shapes)
shelf_obj_poses = generate_poses_and_place_shelf_objs(shelf_objects, shelf_regions, self.env)
self.kitchen_region = AARegion('kitchen_region',
((-x_extents + 2, x_extents * 0.2 + 2), (1 - y_extents / 2, 1 + y_extents / 2)),
z=0.135, color=np.array((1, 1, 0, 0.25)))
set_xy(self.env.GetKinBody('computer_table'), 5.5, -2.5)
x_size = x_extents
y_size = y_extents
self.entire_region_xy = [x_size / 2.0, 0]
self.entire_region_xy_extents = [x_size, y_size]
self.entire_region = AARegion('entire_region',
((-x_size + self.entire_region_xy[0], x_size + self.entire_region_xy[0]),
(-y_size, y_size)), z=0.135, color=np.array((1, 1, 0, 0.25)))
# allow it to be entire region
# but if not in home_region or in forbidden region, reject in motion planning stage
self.home_region_xy = [x_extents / 2.0, 0]
self.home_region_xy_extents = [x_extents,y_extents]
self.home_region = AARegion('entire_region', ((-x_extents + self.home_region_xy[0], x_extents + self.home_region_xy[0]), (-x_extents, y_extents)), z=0.135, color=np.array((1, 1, 0, 0.25)))
#randomly_place_region(self.env, self.robot, self.home_region)
# randomly placed objects
kitchen_chairs = [body for body in self.env.GetBodies() if body.GetName().find('chair') != -1
and body.GetName().find('computer') == -1]
packing_boxes = [b for b in self.env.GetBodies() if b.GetName().find('packing_box') != -1]
computer_chair = self.env.GetKinBody('computer_chair')
table = self.env.GetKinBody('table')
# kitchen table location
set_obj_xytheta([2, -3.2, -np.pi/2], env.GetKinBody('computer_table'))
set_obj_xytheta([1, -3.2, 0], env.GetKinBody('computer_chair'))
self.is_new_env = problem_config is None
# computer chair location
# place other objects
place_objs_in_region(packing_boxes, self.kitchen_region, self.env)
place_objs_in_region([self.robot], self.entire_region, self.env)
place_objs_in_region(kitchen_chairs, self.kitchen_region, self.env)
self.forbidden_region_xy = [x_extents+0.3, 0]
self.forbidden_region_xy_extents = [x_extents/2.0-0.3, y_extents]
self.forbidden_region = AARegion('forbidden_region',
((-self.forbidden_region_xy_extents[0] + self.forbidden_region_xy[0],
self.forbidden_region_xy_extents[0] + self.forbidden_region_xy[0]),
(-y_extents, y_extents)), z=0.135, color=np.array((1, 1, 0, 0.25)))
robot=self.robot
# left arm IK
robot.SetActiveManipulator('leftarm')
manip = robot.GetActiveManipulator()
ee = manip.GetEndEffector()
ikmodel1 = databases.inversekinematics.InverseKinematicsModel(robot=robot,
iktype=IkParameterization.Type.Transform6D,
forceikfast=True, freeindices=None,
freejoints=None, manip=None)
if not ikmodel1.load():
ikmodel1.autogenerate()
# right arm torso IK
robot.SetActiveManipulator('rightarm_torso')
manip = robot.GetActiveManipulator()
ee = manip.GetEndEffector()
ikmodel2 = databases.inversekinematics.InverseKinematicsModel(robot=robot, \
iktype=IkParameterization.Type.Transform6D, \
forceikfast=True, freeindices=None, \
freejoints=None, manip=None)
if not ikmodel2.load():
ikmodel2.autogenerate()
self.robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1])
self.movable_objects = [computer_chair] + packing_boxes + kitchen_chairs
self.problem_idx = problem_idx
self.goal_base_config = np.array([-0,2.5,np.pi/2.])
self.init_base_config = np.array([-1.5, -1.5, np.pi/2.])
set_robot_config(self.init_base_config, self.robot)
set_obj_xytheta([3, 2, 43 * np.pi / 180.], env.GetKinBody('shelf1'))
set_obj_xytheta([3, -1.8, 43 * np.pi / 180.], env.GetKinBody('shelf2'))
set_obj_xytheta([-0.13, 0.45, 3], packing_boxes[2])
self.set_obj_poses(problem_idx)
if self.problem_idx == 1:
env.Remove(env.GetKinBody('shelf1'))
env.Remove(env.GetKinBody('shelf2'))
else:
wall_in_kitchen = box_body(env, 0.04 * 2, 1.3, 2, name='wall_in_kitchen', color=(0, 0, 0))
place_body(env, wall_in_kitchen, (2.1, 0, np.pi / 2), base_name='bottom_wall')
def rearrangement_problem(env,obj_poses=None):
# import random
# seed = 50
# random.seed(seed)
# np.random.seed(seed)
MIN_DELTA = .01 # .01 | .02
ENV_FILENAME = ENVIRONMENTS_DIR+'/single_table.xml'
env.Load(ENV_FILENAME)
robot = env.GetRobots()[0]
TABLE_NAME = 'table1'
NUM_OBJECTS = 8
WIDTH = .05 # .07 | .1
TARGET_WIDTH = 0.07
OBJ_HEIGHT = 0.2
objects = [box_body(env, WIDTH, WIDTH, .2, name='obj%s'%i, \
color=(0, (i+.5)/NUM_OBJECTS, 0)) for i in range(NUM_OBJECTS)]
target_obj = box_body(env, TARGET_WIDTH, TARGET_WIDTH, .2, name='target_obj', \
color=(1, 1.5, 0))
#TODO: Place obstacle that prevent you to reach from top
OBST_X = 0
OBST_WIDTH = 1.
OBST_COLOR = (1, 0, 0)
OBST_HEIGHT = 0.4
OBST_TRANSPARENCY = .25
place_body(env, box_body(env, .4, .05, OBST_HEIGHT, name='obst1', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
(.0, OBST_X-(OBST_WIDTH-.05)/2, 0), TABLE_NAME)
place_body(env, box_body(env, .4, .05, OBST_HEIGHT, name='obst2', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
(.0, OBST_X+(OBST_WIDTH-.05)/2, 0), TABLE_NAME)
place_body(env, box_body(env, .05, OBST_WIDTH, OBST_HEIGHT, name='obst3', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
(.225, OBST_X, 0), TABLE_NAME)
# roof
OBST_Z = OBST_HEIGHT
place_xyz_body(env, box_body(env, .4, OBST_WIDTH, .01, name='obst4', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
(0, OBST_X, OBST_Z,0), TABLE_NAME)
# I think this can be done once and for all
REST_TORSO = [.15]
robot.SetDOFValues(REST_TORSO, [robot.GetJointIndex('torso_lift_joint')])
l_model = databases.linkstatistics.LinkStatisticsModel(robot)
if not l_model.load(): l_model.autogenerate()
l_model.setRobotWeights()
min_delta = 0.01
l_model.setRobotResolutions(xyzdelta=min_delta) # xyzdelta is the minimum Cartesian distance of an object
extrema = aabb_extrema(aabb_union([aabb_from_body(body) for body in env.GetBodies()])).T
robot.SetAffineTranslationLimits(*extrema)
X_PERCENT = 0.0
# define intial region for movable objects
init_region = create_region(env, 'init_region', ((-0.8, 0.8), (-0.7, 0.6)), TABLE_NAME, color=np.array((1, 0, 1, .5)))
#init_region.draw(env)
# define target object region
target_obj_region = create_region(env, 'target_obj_region', ((-0.2, 0.6), (-0.5,0.5)), \
TABLE_NAME, color=np.array((0, 0, 0, 0.9)))
target_obj_region.draw(env)
# place object
if obj_poses is not None:
for obj in objects:
set_pose(obj,obj_poses[get_name(obj)])
set_quat(obj, quat_from_z_rot(0))
if env.GetKinBody(get_name(obj)) is None: env.Add(obj)
set_pose( target_obj, obj_poses[get_name(target_obj)] )
set_quat( target_obj, quat_from_z_rot(0) )
if env.GetKinBody(get_name(target_obj)) is None: env.Add(target_obj)
else:
for obj in objects:
randomly_place_region(env, obj, init_region)
set_quat(obj, quat_from_z_rot(0))
randomly_place_region(env, target_obj, target_obj_region)
set_quat(target_obj, quat_from_z_rot(0))
object_names = [get_name(body) for body in objects] # (tothefront,totheback), (to_the_right,to_the_left)
set_xy(robot, -.75, 0)
# LEFT_ARM_CONFIG = HOLDING_LEFT_ARM
robot.SetDOFValues(REST_LEFT_ARM, robot.GetManipulator('leftarm').GetArmIndices())
robot.SetDOFValues(mirror_arm_config(REST_LEFT_ARM), robot.GetManipulator('rightarm').GetArmIndices())
grasps = {}
for obj_name in object_names:
obj = env.GetKinBody(obj_name)
obj_grasps = get_grasps(env, robot, obj, GRASP_APPROACHES.SIDE, GRASP_TYPES.TOUCH) \
+ get_grasps(env, robot, obj, GRASP_APPROACHES.TOP, GRASP_TYPES.TOUCH)
grasps[get_name(obj)] = obj_grasps
target_obj = env.GetKinBody('target_obj')
target_obj_grasps = get_grasps(env, robot, target_obj, GRASP_APPROACHES.TOP, GRASP_TYPES.TOUCH)+\
get_grasps(env, robot, target_obj, GRASP_APPROACHES.SIDE, GRASP_TYPES.TOUCH)
grasps['target_obj'] = target_obj_grasps
initial_poses={}
for obj in objects:
initial_poses[get_name(obj)] = get_pose(obj)
initial_poses[get_name(target_obj)] = get_pose(target_obj)
return ManipulationProblem(None,
object_names=object_names, table_names='table1',
goal_regions=init_region,grasps=grasps,
initial_poses = initial_poses)
