def stacking(env):
env.Load(ENVIRONMENTS_DIR + 'room1.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']
regions = [
create_region(env, 'blue_goal', ((-1, 1), (-1, 0)), 'table1', color=np.array((0, 0, 1, .5))),
create_region(env, 'green_goal', ((-1, -.5), (-1, 1)), 'table2', color=np.array((0, 1, 0, .5)))
]
pb = place_body
pb(env, box_body(env, .07, .07, .2, name='blue_box', color=BLUE), (1.65, 0.115, PI/3), 'table1')
pb(env, box_body(env, .07, .07, .2, name='green_box', color=GREEN), (1.55, -0.215, 7*PI/6), 'table1')
pb(env, box_body(env, .07, .07, .2, name='purple_box', color=BLACK), (.15, -1.8, 7*PI/6), 'table2')
pb(env, box_body(env, .07, .07, .2, name='red_box1', color=RED), (1.55, -0.215, 0), 'green_box')
pb(env, box_body(env, .07, .07, .2, name='red_box2', color=RED), (1.65, .6, PI/4), 'table1')
pb(env, box_body(env, .07, .07, .2, name='red_box3', color=RED), (.3, -1.9, PI/12), 'table2')
object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names]
goal_stackings = {
'purple_box': 'blue_box' # target_cylinder
}
goal_regions = {
'blue_box': 'blue_goal',
'green_box': 'green_goal'
}
return ManipulationProblem(function_name(inspect.stack()), camera_trans=camera_trans,
object_names=object_names, table_names=table_names, regions=regions,
goal_stackings=goal_stackings, goal_regions=goal_regions)
def dinner(env):
env.Load(ENVIRONMENTS_DIR + 'kitchen.xml')
obstacle_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()]
table_names = ['table1']
sink_names = ['washer']
stove_names = ['heater']
regions = [
create_region(env, 'blue_goal', ((-1, 1), (-1, -.5)), 'table1', color=np.array((0, 0, 1, .5))),
create_region(env, 'green_goal', ((-1, 1), (.5, 1)), 'table1', color=np.array((0, 1, 0, .5)))
]
pb = place_body
pb(env, box_body(env, .07, .07, .2, name='blue', color=BLUE), (1.65, 0.115, PI/3), 'table1')
pb(env, box_body(env, .07, .07, .2, name='green', color=GREEN), (1.55, -0.215, 7*PI/6), 'table1')
pb(env, box_body(env, .07, .07, .2, name='red1', color=RED), (1.6, 0.615, 0), 'table1')
pb(env, box_body(env, .07, .07, .2, name='red2', color=RED), (1.7, -0.415, PI/8), 'table1')
object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names]
goal_regions = {
'blue': 'blue_goal',
'green': 'green_goal'
}
goal_cleaned = ['blue']
goal_cooked = ['green']
return ManipulationProblem(function_name(inspect.stack()),
object_names=object_names, table_names=table_names, sink_names=sink_names, stove_names=stove_names, regions=regions,
goal_regions=goal_regions, goal_cleaned=goal_cleaned, goal_cooked=goal_cooked)
def kitchen(env):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'kitchen.xml')
obstacle_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
table_names = ['table1']
sink_names = ['washer']
#stove_names = ['heater']
stove_names = ['microwave']
regions = [
create_region(env,
'blue_goal', ((-1, 1), (-1, -.5)),
'table1',
color=np.array((0, 0, 1, .5))),
create_region(env,
'green_goal', ((-1, 1), (.5, 1)),
'table1',
color=np.array((0, 1, 0, .5)))
]
pb = place_body
pb(env, box_body(env, .07, .07, .2, name='blue', color=BLUE),
(1.65, 0.115, PI / 3), 'table1')
pb(env, box_body(env, .07, .07, .2, name='green', color=GREEN),
(1.55, -0.215, 7 * PI / 6), 'table1')
pb(env, box_body(env, .07, .07, .2, name='red1', color=RED),
(1.6, 0.615, 0), 'table1')
pb(env, box_body(env, .07, .07, .2, name='red2', color=RED),
(1.7, -0.415, PI / 8), 'table1')
object_names = [
str(body.GetName()) for body in env.GetBodies()
if not body.IsRobot() and str(body.GetName()) not in obstacle_names
]
goal_regions = {'blue': 'blue_goal', 'green': 'green_goal'}
goal_cleaned = ['blue']
goal_cooked = ['green']
return ManipulationProblem(function_name(inspect.stack()),
object_names=object_names,
table_names=table_names,
sink_names=sink_names,
stove_names=stove_names,
regions=regions,
goal_regions=goal_regions,
goal_cleaned=goal_cleaned,
goal_cooked=goal_cooked)
def stacking(env):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'room1.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']
regions = [
create_region(env,
'blue_goal', ((-1, 1), (-1, 0)),
'table1',
color=np.array((0, 0, 1, .5))),
create_region(env,
'green_goal', ((-1, -.5), (-1, 1)),
'table2',
color=np.array((0, 1, 0, .5)))
]
pb = place_body
pb(env, box_body(env, .07, .07, .2, name='blue_box', color=BLUE),
(1.65, 0.115, PI / 3), 'table1')
pb(env, box_body(env, .07, .07, .2, name='green_box', color=GREEN),
(1.55, -0.215, 7 * PI / 6), 'table1')
pb(env, box_body(env, .07, .07, .2, name='purple_box', color=BLACK),
(.15, -1.8, 7 * PI / 6), 'table2')
pb(env, box_body(env, .07, .07, .2, name='red_box1', color=RED),
(1.55, -0.215, 0), 'green_box')
#pb(env, box_body(env, .07, .07, .2, name='red_box2', color=RED), (1.65, .6, PI/4), 'table1')
#pb(env, box_body(env, .07, .07, .2, name='red_box3', color=RED), (.3, -1.9, PI/12), 'table2')
object_names = [
str(body.GetName()) for body in env.GetBodies()
if not body.IsRobot() and str(body.GetName()) not in obstacle_names
]
goal_stackings = {
# 'purple_box': 'blue_box' # target_cylinder
}
goal_regions = {'blue_box': 'blue_goal', 'green_box': 'green_goal'}
return ManipulationProblem(function_name(inspect.stack()),
camera_trans=camera_trans,
object_names=object_names,
table_names=table_names,
regions=regions,
goal_stackings=goal_stackings,
goal_regions=goal_regions)
def distractions(env):
env.Load(ENVIRONMENTS_DIR + 'room1.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']
regions = [
create_region(env, 'green_goal', ((-1, 1), (-1, 0)), 'table1', color=np.array((0, 1, 0, .5))),
]
pb = place_body
center = np.array([1.75, 0.25, 0])
pb(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN), center, 'table1')
id = count(1)
for dx, dy in product((-.15, 0, .15), repeat=2):
if not (dx == 0 and dy == 0):
pb(env, box_body(env, .07, .05, .2, name='red_box%d'%next(id), color=RED), center + np.array([dx, dy, 0]), 'table1')
#for x in irange(-.7, .7, .15):
# for y in irange(-2.0, -1.5, .15):
for x in irange(-.65, .75, .15):
for y in irange(-1.95, -1.5, .15):
pb(env, box_body(env, .05, .05, .15, name='red_box%d'%next(id), color=RED), np.array([x, y, 0]), 'table2')
object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names]
goal_regions = {
'green_box': 'green_goal',
}
return ManipulationProblem(function_name(inspect.stack()), camera_trans=camera_trans,
object_names=object_names, table_names=table_names, regions=regions,
goal_regions=goal_regions)
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 distract_n(env, n):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'room1.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']
regions = [
create_region(env,
'green_goal', ((-1, 1), (-1, 0)),
'table1',
color=np.array((0, 1, 0, .5))),
]
pb = place_body
center = np.array([1.75, 0.25, 0])
pb(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN), center,
'table1')
id = count(1)
for dx, dy in [(-.15, 0), (.15, 0), (0, -.15), (0, .15)]:
pb(env,
box_body(env, .07, .05, .2, name='red_box%d' % next(id), color=RED),
center + np.array([dx, dy, 0]), 'table1')
#for x in irange(-.7, .7, .15):
# for y in irange(-2.0, -1.5, .15):
for x, y in islice(
product(irange(-.65, .75, .15), irange(-1.95, -1.5, .15)), n):
pb(env,
box_body(env, .07, .05, .2, name='red_box%d' % next(id), color=RED),
np.array([x, y, 0]), 'table2')
object_names = [
str(body.GetName()) for body in env.GetBodies()
if not body.IsRobot() and str(body.GetName()) not in obstacle_names
]
goal_regions = {
'green_box': 'green_goal',
}
return ManipulationProblem(function_name(inspect.stack()),
camera_trans=camera_trans,
object_names=object_names,
table_names=table_names,
regions=regions,
goal_regions=goal_regions)
def simple2(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']
table_names = ['table1', 'table2']
regions = [
create_region(env,
'blue_goal', ((-1, 1), (-1, 0)),
'table1',
color=np.array((0, 0, 1, .5))),
create_region(env,
'green_goal', ((-1, -.5), (-1, 1)),
'table2',
color=np.array((0, 1, 0, .5)))
]
pb = place_body
pb(env, box_body(env, .07, .05, .2, name='blue_box', color=BLUE),
(1.65, 0.115, PI / 3), 'table1')
pb(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN),
(1.55, -0.215, 7 * PI / 6), 'table1')
object_names = [
str(body.GetName()) for body in env.GetBodies()
if not body.IsRobot() and str(body.GetName()) not in obstacle_names
]
goal_regions = {'blue_box': 'blue_goal', 'green_box': 'green_goal'}
return ManipulationProblem(function_name(inspect.stack()),
object_names=object_names,
table_names=table_names,
regions=regions,
goal_regions=goal_regions)
def simple(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']
regions = [
create_region(env, 'blue_goal', ((-1, 1), (-1, 0)), 'table1', color=np.array((0, 0, 1, .5))),
create_region(env, 'green_goal', ((-1, -.5), (-1, 1)), 'table2', color=np.array((0, 1, 0, .5)))
]
pb = place_body
pb(env, box_body(env, .07, .05, .2, name='blue_box', color=BLUE), (1.65, 0.115, PI/3), 'table1')
pb(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN), (1.55, -0.215, 7*PI/6), 'table1')
object_names = [str(body.GetName()) for body in env.GetBodies() if not body.IsRobot() and str(body.GetName()) not in obstacle_names]
goal_regions = {
'blue_box': 'blue_goal',
'green_box': 'green_goal'
}
return ManipulationProblem(function_name(inspect.stack()),
object_names=object_names, table_names=table_names, regions=regions,
goal_regions=goal_regions)
def two_tables_through_door(env,
obj_shapes=None,
obj_poses=None,
obst_shapes=None,
obst_poses=None):
env.Load('env.xml')
robot = env.GetRobots()[0]
set_default_robot_config(robot)
region = create_region(env, 'goal', ((-1, 1), (-.3, .3)), \
'floorwalls', color=np.array((0, 0, 1, .25)))
set_config(robot, FOLDED_LEFT_ARM,
robot.GetManipulator('leftarm').GetArmIndices())
set_config(robot,mirror_arm_config(FOLDED_LEFT_ARM),\
robot.GetManipulator('rightarm').GetArmIndices())
# 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()
# loading areas
init_loading_region = AARegion('init_loading_area',\
((-2.51,-0.81),(-2.51,0)),\
z=0.0001,color=np.array((1,0,1,0.25)))
init_loading_region.draw(env)
init_loading_region2 = AARegion('init_loading_area2',\
((-2.51,-0.81),(1.7,2.6)),\
z=0.0001,color=np.array((1,0,1,0.25)))
init_loading_region2.draw(env)
init_loading_region4 = AARegion('init_loading_area4',\
((-2.51,-1.5),(-0.1,2)),\
z=0.0001,color=np.array((1,0,1,0.25)))
init_loading_region4.draw(env)
loading_regions =[init_loading_region,init_loading_region2,\
init_loading_region4]
loading_region = AARegion('loading_area',\
((-2.51,-0.81),(-2.51,2.51)),\
z=0.0001,color=np.array((1,1,0,0.25)))
loading_region.draw(env)
# converyor belt region
conv_x = 2
conv_y = 1
conveyor_belt = AARegion('conveyor_belt',\
((-1+conv_x,10*max_width+conv_x),\
(-0.4+conv_y,0.5+conv_y)),\
z=0.0001,color=np.array((1,0,0,0.25)))
conveyor_belt.draw(env)
all_region = AARegion('all_region',\
((-2.51,10*max_width+conv_x),(-3.51,3.51)),\
z=0.0001,color=np.array((1,1,0,0.25)))
if obj_shapes == None:
OBJECTS, obj_shapes, obj_poses = create_objects(env, conveyor_belt)
else:
OBJECTS = load_objects(env, obj_shapes, obj_poses, color=(0, 1, 0))
if obst_shapes == None:
OBSTACLES, obst_shapes, obst_poses = create_obstacles(
env, loading_regions)
else:
OBSTACLES = load_objects(env, obst_shapes, obst_poses, color=(0, 0, 1))
initial_saver = DynamicEnvironmentStateSaver(env)
initial_state = (initial_saver, [])
init_base_conf = np.array([0, 1.05, 0])
problem = {'initial_state':initial_state,\
'obstacles':OBSTACLES,\
'objects':OBJECTS,\
'loading_region':loading_region,\
'env':env,\
'obst_shapes':obst_shapes,\
'obst_poses':obst_poses,\
'obj_shapes':obj_shapes,\
'obj_poses':obj_poses,\
'all_region':all_region,\
'init_base_conf':init_base_conf}
return problem # the second is for indicating 0 placed objs
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)
def two_tables_through_door(env): # Previously 4, 8
env.Load('env.xml')
robot = env.GetRobots()[0]
set_default_robot_config(robot)
region = create_region(env, 'goal', ((-1, 1), (-.3, .3)), \
'floorwalls', color=np.array((0, 0, 1, .25)))
set_config(robot, FOLDED_LEFT_ARM,
robot.GetManipulator('leftarm').GetArmIndices())
set_config(robot,mirror_arm_config(FOLDED_LEFT_ARM),\
robot.GetManipulator('rightarm').GetArmIndices())
# 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()
# obj definitions
min_height = 0.4
max_height = 1
min_width = 0.2
max_width = 0.6
min_length = 0.2
max_length = 0.6
# loading areas
#rightmost one
init_loading_region = AARegion('init_loading_area',
((-2.51, -0.81), (-2.51, -1)),
z=0.0001,
color=np.array((1, 0, 1, 0.25)))
init_loading_region.draw(env)
init_loading_region2 = AARegion('init_loading_area2',
((-2.51, -0.81), (1.7, 2.6)),
z=0.0001,
color=np.array((1, 0, 1, 0.25)))
init_loading_region2.draw(env)
init_loading_region3 = AARegion('init_loading_area3',
((-1.3, -0.81), (-1, 0)),
z=0.0001,
color=np.array((1, 0, 1, 0.25)))
init_loading_region3.draw(env)
init_loading_region4 = AARegion('init_loading_area4',
((-2.51, -2), (-1, 0)),
z=0.0001,
color=np.array((1, 0, 1, 0.25)))
init_loading_region4.draw(env)
loading_regions =[init_loading_region,init_loading_region2,\
init_loading_region3,init_loading_region4]
loading_region = AARegion('loading_area', ((-2.51, -0.81), (-2.51, 2.51)),
z=0.0001,
color=np.array((1, 1, 0, 0.25)))
loading_region.draw(env)
# converyor belt region
conv_x = 2
conv_y = 1
conveyor_belt = AARegion('conveyor_belt',
((-1 + conv_x, 10 * max_width + conv_x),
(-0.4 + conv_y, 0.5 + conv_y)),
z=0.0001,
color=np.array((1, 0, 0, 0.25)))
conveyor_belt.draw(env)
all_region = AARegion('all_region',
((-2.51, 10 * max_width + conv_x), (-3.51, 3.51)),
z=0.0001,
color=np.array((1, 1, 0, 0.25)))
"""
obj1 = box_body(env,0.5,0.5,0.5,\
name='obst1',\
color=(0, 1, 1))
env.Add(obj1)
obj2 = box_body(env,0.5,0.5,0.5,\
name='obst2',\
color=(0, 1, 1))
env.Add(obj2)
set_point(obj1,[-1,-1,0.75])
set_point(obj1,[-1.9,-0.5,0.01])
set_point(obj2,[-1.,-0.5,0.01])
set_point(obj2,[-1,0.7,0.01])
"""
NUM_OBSTACLES = 4
OBSTACLES = []
obstacle_poses = {}
obstacle_shapes = {}
i = 0
for i in range(NUM_OBSTACLES):
width = np.random.rand(1) * (max_width - min_width) + min_width
length = np.random.rand(1) * (max_length - min_length) + min_length
height = np.random.rand(1) * (max_height - min_height) + min_height
trans = np.eye(4)
trans[2, -1] = 0.075
new_body = box_body(env,width,length,height,\
name='obj%s'%i,\
color=(0, (i+.5)/NUM_OBSTACLES, 0))
env.Add(new_body)
new_body.SetTransform(trans)
xytheta = randomly_place_in_region(
env, new_body, loading_regions[np.random.randint(4)])
if not (xytheta is None):
obstacle_shapes['obst%s' %
len(OBSTACLES)] = [width[0], length[0], height[0]]
obstacle_poses['obst%s' % len(OBSTACLES)] = xytheta
OBSTACLES.append(new_body)
else:
env.Remove(new_body)
goal_base_pose = np.array([-2, -2, 5 * PI / 4])
robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis,
[0, 0, 1])
import pdb
pdb.set_trace()
n_node_lim_list = [3000, 4000, 5000, 6000, 7000] #,8000,9000,1000]
stime = time.time()
n_node_lim = np.inf
for n_node_lim in n_node_lim_list:
path, tpath2, status2 = get_motion_plan(robot,
goal_base_pose,
env,
maxiter=20,
n_node_lim=n_node_lim)
if status2 is "HasSolution":
print n_node_lim
break
print time.time() - stime
import pdb
pdb.set_trace()
set_robot_config(goal_base_pose, robot)
"""