def two_arm_place_object(obj, robot, place_action):
place_base_pose = place_action['base_pose']
leftarm_manip = robot.GetManipulator('leftarm')
rightarm_manip = robot.GetManipulator('rightarm')
set_robot_config(place_base_pose, robot)
release_obj(robot, obj)
FOLDED_LEFT_ARM = [0.0, 1.29023451, 0.0, -2.121308, 0.0, -0.69800004, 0.0]
set_config(robot, FOLDED_LEFT_ARM, leftarm_manip.GetArmIndices())
set_config(robot, mirror_arm_config(FOLDED_LEFT_ARM), rightarm_manip.GetArmIndices())
def fold_arms():
assert len(openravepy.RaveGetEnvironments()) == 1
env = openravepy.RaveGetEnvironments()[0]
robot = env.GetRobot('pr2')
leftarm_manip = robot.GetManipulator('leftarm')
rightarm_manip = robot.GetManipulator('rightarm')
FOLDED_LEFT_ARM = [0.0, 1.29023451, 0.0, -2.12, 0.0, -0.69800004, 0.0]
set_config(robot, FOLDED_LEFT_ARM, leftarm_manip.GetArmIndices())
set_config(robot, mirror_arm_config(FOLDED_LEFT_ARM), rightarm_manip.GetArmIndices())
def two_arm_place_object(place_action):
assert len(openravepy.RaveGetEnvironments()) == 1
env = openravepy.RaveGetEnvironments()[0]
robot = env.GetRobot('pr2')
try:
place_base_pose = place_action['q_goal']
except KeyError:
place_base_pose = place_action['base_pose']
leftarm_manip = robot.GetManipulator('leftarm')
rightarm_manip = robot.GetManipulator('rightarm')
set_robot_config(place_base_pose, robot)
release_obj()
set_config(robot, FOLDED_LEFT_ARM, leftarm_manip.GetArmIndices())
set_config(robot, mirror_arm_config(FOLDED_LEFT_ARM), rightarm_manip.GetArmIndices())
def create_conveyor_belt_problem(env, obj_setup=None):
if obj_setup is not None:
obj_shapes = obj_setup['object_shapes']
obj_poses = obj_setup['object_poses']
obst_shapes = obj_setup['obst_shapes']
obst_poses = obj_setup['obst_poses']
fdir = os.path.dirname(os.path.abspath(__file__))
env.Load(fdir + '/convbelt_env.xml')
robot = env.GetRobots()[0]
set_default_robot_config(robot)
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()
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()
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
loading_region = AARegion('loading_area', ((-3.51, -0.81), (-2.51, 2.51)),
z=0.01,
color=np.array((1, 1, 0, 0.25)))
# converyor belt region
conv_x = 3
conv_y = 1
conveyor_belt = AARegion('conveyor_belt',
((-1 + conv_x, 20 * max_width + conv_x),
(-0.4 + conv_y, 0.5 + conv_y)),
z=0.01,
color=np.array((1, 0, 0, 0.25)))
all_region = AARegion('all_region',
((-3.51, 20 * max_width + conv_x), (-2.51, 2.51)),
z=0.01,
color=np.array((1, 1, 0, 0.25)))
if obj_setup is None:
objects, obj_shapes, obj_poses = create_objects(env, conveyor_belt)
obstacles, obst_shapes, obst_poses = create_obstacles(
env, loading_region)
else:
objects = load_objects(env, obj_shapes, obj_poses, color=(0, 1, 0))
obstacles = load_objects(env, obst_shapes, obst_poses, color=(0, 0, 1))
initial_saver = DynamicEnvironmentStateSaver(env)
initial_state = (initial_saver, [])
#set_obj_xytheta([-1, -1, 1], obstacles[0])
#set_obj_xytheta([-2, 2.3, 0], obstacles[1])
init_base_conf = np.array([0, 1.05, 0])
set_robot_config(np.array([0, 1.05, 0]), robot)
#obst_poses = [randomly_place_in_region(env, obj, loading_region) for obj in obstacles]
#obst_poses = [get_body_xytheta(obj) for obj in obstacles]
"""
tobj = env.GetKinBody('tobj3')
tobj_xytheta = get_body_xytheta(tobj.GetLinks()[1])
tobj_xytheta[0, -1] = (160 / 180.0) * np.pi
set_obj_xytheta(tobj_xytheta, tobj.GetLinks()[1])
objects = []
for tobj in env.GetBodies():
if tobj.GetName().find('tobj') == -1: continue
randomly_place_in_region(env, tobj, conveyor_belt)
objects.append(tobj)
"""
problem = {
'initial_state': initial_state,
'obstacles': obstacles,
'objects': objects,
'conveyor_belt_region': conveyor_belt,
'loading_region': loading_region,
'env': env,
'obst_shapes': obst_shapes,
'obst_poses': obst_poses,
'obj_shapes': obj_shapes,
'obj_poses': obj_poses,
'entire_region': all_region,
'init_base_conf': init_base_conf
}
return problem # the second is for indicating 0 placed objs
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 create_conveyor_belt_problem(env, obj_setup=None, problem_idx=0):
if obj_setup is not None:
obj_shapes = obj_setup['object_shapes']
obj_poses = obj_setup['object_poses']
obst_shapes = obj_setup['obst_shapes']
obst_poses = obj_setup['obst_poses']
fdir = os.path.dirname(os.path.abspath(__file__))
if problem_idx == 0 or problem_idx == 1:
env.Load(fdir + '/convbelt_env_diffcult_shapes.xml')
else:
env.Load(fdir + '/convbelt_env_diffcult_shapes_two_rooms.xml')
"""
if problem_idx == 0:
env.Load(fdir + '/convbelt_env_diffcult_shapes.xml')
else:
env.Load(fdir + '/convbelt_env.xml')
"""
robot = env.GetRobots()[0]
set_default_robot_config(robot)
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')
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')
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
"""
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]),
(-y_extents, y_extents)), z=0.135, color=np.array((1, 1, 0, 0.25)))
"""
init_base_conf = np.array([0, 1.05, 0])
set_robot_config(np.array([0, 1.05, 0]), robot)
# converyor belt region
conv_x = 3
conv_y = 1
conveyor_belt = AARegion('conveyor_belt',
((-1 + conv_x, 20 * max_width + conv_x),
(-0.4 + conv_y, 0.5 + conv_y)),
z=0.01,
color=np.array((1, 0, 0, 0.25)))
y_extents = 5.0
x_extents = 3.01
entire_region = AARegion('entire_region',
((-7.4, 20 * max_width + conv_x),
(-y_extents - 2.5, y_extents - 2)),
z=0.01,
color=np.array((1, 1, 0, 0.25)))
loading_region = AARegion('loading_area', ((-7.4, -0.5), (-7.5, 3.0)),
z=0.01,
color=np.array((1, 1, 0, 0.25)))
big_region_1 = AARegion('big_region_1', ((-5, -0.5), (-7.5, -0.4)),
z=0.01,
color=np.array((1, 1, 0, 0.25)))
big_region_2 = AARegion('big_region_2', ((-7.4, -4.0), (-7.5, 3.0)),
z=0.01,
color=np.array((1, 1, 0, 0.25)))
if problem_idx == 0 or problem_idx == 1:
objects = []
i = 1
for tobj in env.GetBodies():
if tobj.GetName().find('tobj') == -1: continue
randomly_place_in_region(env, tobj, conveyor_belt)
set_obj_xytheta([2 + i, 1.05, 0], tobj)
objects.append(tobj)
i += 1.1
square_objects, obj_shapes, obj_poses = create_objects(env,
conveyor_belt,
num_objects=10)
objects += square_objects
else:
objects = []
i = 1
for tobj in env.GetBodies():
if tobj.GetName().find('tobj') == -1: continue
randomly_place_in_region(env, tobj, conveyor_belt)
set_obj_xytheta([2 + i, 1.05, get_body_xytheta(tobj)[0, -1]], tobj)
#objects.append(tobj)
i += 1.1
square_objects, obj_shapes, obj_poses = create_objects(env,
conveyor_belt,
num_objects=20)
for obj in square_objects:
set_obj_xytheta([2 + i, 1.05, get_body_xytheta(obj)[0, -1]], obj)
objects.append(obj)
i += 1.1
#objects += square_objects
"""
if problem_idx == 0:
objects = []
i = 1
for tobj in env.GetBodies():
if tobj.GetName().find('tobj') == -1: continue
randomly_place_in_region(env, tobj, conveyor_belt)
set_obj_xytheta([2+i, 1.05, 0], tobj)
objects.append(tobj)
i += 1.1
square_objects, obj_shapes, obj_poses = create_objects(env, conveyor_belt, num_objects=10)
objects += square_objects
else:
objects, obj_shapes, obj_poses = create_objects(env, conveyor_belt, num_objects=20)
"""
initial_saver = DynamicEnvironmentStateSaver(env)
initial_state = (initial_saver, [])
problem = {
'initial_state': initial_state,
'objects': objects,
'conveyor_belt_region': conveyor_belt,
'loading_region': loading_region,
'big_region_1': big_region_1,
'big_region_2': big_region_2,
'env': env,
'entire_region': entire_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 main():
env=Environment()
env.Load('env.xml')
robot = env.GetRobots()[0]
#set_default_robot_config(robot)
robot_initial_pose = get_pose(robot)
leftarm_manip = robot.GetManipulator('leftarm')
robot.SetActiveManipulator('leftarm')
ikmodel1 = databases.inversekinematics.InverseKinematicsModel(robot=robot, \
iktype=IkParameterization.Type.Transform6D, \
forceikfast=True, freeindices=None, \
freejoints=None, manip=None)
if not ikmodel1.load():
ikmodel1.autogenerate()
rightarm_manip = robot.GetManipulator('rightarm')
rightarm_torso_manip = robot.GetManipulator('rightarm_torso')
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()
set_config(robot,FOLDED_LEFT_ARM,robot.GetManipulator('leftarm').GetArmIndices())
set_config(robot,mirror_arm_config(FOLDED_LEFT_ARM),\
robot.GetManipulator('rightarm').GetArmIndices())
robot_initial_config = np.array([-1,1,0])
set_robot_config(robot_initial_config,robot)
env.SetViewer('qtcoin')
target = env.GetKinBody('target')
width = 0.1 #np.random.rand(1)*(max_width-min_width)+min_width
length = 0.8 #np.random.rand(1)*(max_length-min_length) + min_length
height = 1.0 #np.random.rand(1)*(max_height-min_height)+min_height
new_body = box_body(env,width,length,height,\
name='obst',\
color=(0, 5, 1))
trans = np.eye(4);
trans[2,-1] = 0.075
#env.Add(new_body);
#new_body.SetTransform(trans)
all_region = region = AARegion('all_region',((-2.51,2.51),(-2.51,2.51)),\
z=0.0001,color=np.array((1,1,0,0.25)))
activate_arms_torso_base(robot)
# Try picking different objects
pick_obj = lambda obj_name: pick(obj_name,env,robot,all_region)
import pdb;pdb.set_trace()
pbase,grasp,g_config = sample_pick(env.GetRobot('valkyrie'),robot,env,all_region)
pick_obj(env.GetRobot('valkyrie'),robot,g_config,leftarm_manip,rightarm_manip)
import pdb;pdb.set_trace()
path,tpath,status = get_motion_plan(robot,robot_xytheta,env,maxiter=10,n_node_lim=5000)
import pdb;pdb.set_trace()
sample_pick(target,robot,env,all_region)
import pdb;pdb.set_trace()
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)
"""