def randomly_place_in_region_need_to_be_fixed(env, obj, region, th=None): # todo fix this function min_x = region.box[0][0] max_x = region.box[0][1] min_y = region.box[1][0] max_y = region.box[1][1] aabb = aabb_from_body(obj) # try 1000 placements for _ in range(300): x = np.random.rand(1) * (max_x - min_x) + min_x y = np.random.rand(1) * (max_y - min_y) + min_y z = [region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET] xyz = np.array([x[0],y[0],z[0]]) - aabb.pos() + get_point(obj) # todo: recheck this function; I think it failed if obj was robot if th == None: th = np.random.rand(1) * 2 * np.pi set_point(obj, xyz) set_quat(obj, quat_from_angle_vector(th, np.array([0, 0, 1]))) obj_quat = get_quat(obj) # refer to conversion between quaternions and euler angles on Wiki for the following eqn. assert (np.isclose(th, np.arccos(obj_quat[0]) * 2) or np.isclose(th, np.arccos(-obj_quat[0]) * 2)) # print not env.CheckCollision(obj) and region.contains(obj.ComputeAABB()) if not env.CheckCollision(obj) \ and region.contains(obj.ComputeAABB()): return [x[0], y[0], th[0]] return None
def create_in_body(body, color=None): aabb = aabb_from_body(body) return AARegion(body.GetName(), ((aabb.pos()[0] - aabb.extents()[0], aabb.pos()[0] + aabb.extents()[0]), (aabb.pos()[1] - aabb.extents()[1], aabb.pos()[1] + aabb.extents()[1])), aabb.pos()[2] - aabb.extents()[2] + BODY_Z_OFFSET, color=color)
def randomly_place_in_region(env, body, region): if env.GetKinBody(get_name(body)) is None: env.Add(body) for i in range(1000): set_quat(body, quat_from_z_rot(uniform(0, 2 * PI))) aabb = aabb_from_body(body) cspace = region.cspace(aabb) if cspace is None: continue set_point(body, np.array([uniform(*cspace_range) for cspace_range in cspace] + [ region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]) - aabb.pos() + get_point(body)) if not env.CheckCollision(body): return get_body_xytheta(body) return None
def randomly_place_body(env, body, table_names): # import pdb; pdb.set_trace() if env.GetKinBody(get_name(body)) is None: env.Add(body) while True: set_quat(body, quat_from_z_rot(uniform(0, 2*PI))) aabb = aabb_from_body(body) region = AARegion.create_on_body(env.GetKinBody(choice(table_names))) cspace = region.cspace(aabb) if cspace is None: continue set_point(body, np.array([uniform(*range) for range in cspace] + [region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]) - aabb.pos() + get_point(body)) if not body_collision(body): return
def gaussian_randomly_place_in_region(env, body, region, center, var): if env.GetKinBody(get_name(body)) is None: env.Add(body) for i in range(1000): xytheta = np.random.normal(center, var) set_obj_xytheta(xytheta, body) if not body_collision(env, body): return xytheta import pdb;pdb.set_trace() for i in range(1000): set_quat(body, quat_from_z_rot(uniform(0, 2 * PI))) aabb = aabb_from_body(body) cspace = region.cspace(aabb) if cspace is None: continue set_point(body, np.array([uniform(*cspace_range) for cspace_range in cspace] + [ region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]) - aabb.pos() + get_point(body)) if not body_collision(env, body): return get_body_xytheta(body) return None
def get_aabb(self, body_name): body = self.env.GetKinBody(body_name) with body: # NOTE - before I didn't have this and it worked for rlplan set_pose(body, unit_pose()) return aabb_from_body(self.env.GetKinBody(body_name))
obj_xy = get_point(obj)[:-1] robot_xy = get_point(robot)[:-1] dist_to_grasp = np.linalg.norm(robot_xy - obj_xy) n_samples = 1 for _ in range(300): robot_xy = sample_base_locations(arm_len, obj, env)[:-1] angle = sample_angle_facing_given_transform(obj_xy, robot_xy) # angle around z set_robot_config(np.r_[robot_xy, angle], robot) if (not env.CheckCollision(robot)) and np.any([r.contains(robot.ComputeAABB()) for r in multiple_regions]): return np.array([robot_xy[0], robot_xy[1], angle]) return None def create_region(env, region_name, ((nx, px), (ny, py)), table_name, color=None): table_aabb = aabb_from_body(env.GetKinBody(table_name)) return AARegion(region_name, ((table_aabb.pos()[0] + nx * table_aabb.extents()[0], table_aabb.pos()[0] + px * table_aabb.extents()[0]), (table_aabb.pos()[1] + ny * table_aabb.extents()[1], table_aabb.pos()[1] + py * table_aabb.extents()[1])), table_aabb.pos()[2] + table_aabb.extents()[2] + REGION_Z_OFFSET, color=color) def simulate_base_path(robot, path): for p in path: # set_config(robot, p, get_active_arm_indices(robot)) set_robot_config(p, robot) sleep(0.001)
def create_on_body(body, color=None): aabb = aabb_from_body(body) return AARegion(body.GetName(), ((aabb.pos()[0]-aabb.extents()[0],aabb.pos()[0]+aabb.extents()[0]), (aabb.pos()[1]-aabb.extents()[1],aabb.pos()[1]+aabb.extents()[1])), aabb.pos()[2]+aabb.extents()[2] + BODY_Z_OFFSET, color=color)
from manipulation.bodies.bounding_volumes import aabb_base_center, aabb_from_body from constants import REGION_Z_OFFSET, BODY_Z_OFFSET import numpy as np # TODO - make static def create_region(env, region_name, ((nx, px), (ny, py)), table_name, color=None): table_aabb = aabb_from_body(env.GetKinBody(table_name)) return AARegion(region_name, ((table_aabb.pos()[0] + nx*table_aabb.extents()[0], table_aabb.pos()[0] + px*table_aabb.extents()[0]), (table_aabb.pos()[1] + ny*table_aabb.extents()[1], table_aabb.pos()[1] + py*table_aabb.extents()[1])), table_aabb.pos()[2] + table_aabb.extents()[2] + REGION_Z_OFFSET, color=color) class AARegion(object): def __init__(self, name, box, z, color=(0,0,0,0)): self.name = name self.box = box self.z = z self.color = color self.draw_handle = None def area(self): return (self.box[0][1] - self.box[0][0])*(self.box[1][1] - self.box[1][0]) def contains_point(self, point): return not (any(self.box[k][0] > point[k] or self.box[k][1] < point[k] \ for k in range(2)) or self.z > point[2]) def contains(self, aabb): return not (any(self.box[k][0] > aabb.pos()[k]-aabb.extents()[k] or self.box[k][1] < aabb.pos()[k]+aabb.extents()[k] \ for k in range(2)) or self.z > aabb.pos()[2]-aabb.extents()[2]) # Saves a couple operations over calling contains_point twice def on_edge(self, aabb): return self.contains_point(aabb_base_center(aabb)) and not self.contains(aabb) def cspace(self, aabb): cspace = [] for k in range(2):
def aabb_collision(body1, body2): return aabb_overlap(aabb_from_body(body1), aabb_from_body(body2))
def aabb_collision(body1, body2): return aabb_overlap(aabb_from_body(body1), aabb_from_body(body2)) def body_collision(body1, body2=None): if body2 is not None: if is_cylinder(body1) and is_cylinder(body2): # TODO - ode cylinder collision checks are having issues return aabb_collision(body1, body2) return get_env().CheckCollision(body1, body2) for body2 in get_env().GetBodies(): if body2 != body1 and body_collision(body1, body2): return True return False def place_body(env, body, (x, y, theta), base_name): # TODO - make it place on the relative table body_aabb = aabb_from_body(body) base_aabb = aabb_from_body(env.GetKinBody(base_name)) z = get_point(body)[2] - (body_aabb.pos()[2] - body_aabb.extents()[2]) + \ (base_aabb.pos()[2] + base_aabb.extents()[2]) + BODY_PLACEMENT_Z_OFFSET set_point(body, (x, y, z)) set_quat(body, quat_from_axis_angle(0, 0, theta)) env.Add(body) def place_body_on_floor(body, (x, y, theta)): aabb = aabb_from_body(body) z = get_point(body)[2] - (aabb.pos()[2] - aabb.extents()[2]) + BODY_PLACEMENT_Z_OFFSET set_point(body, (x, y, z)) set_quat(body, quat_from_axis_angle(0, 0, theta)) get_env().Add(body) def randomly_place_body(env, body, table_names):
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 get_aabb(self, body_name): return aabb_from_body(self.env.GetKinBody(body_name))