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))
