def draw_configs(configs, env, name='point', colors=None, transparency=0.1):
# assert configs[0].shape==(6,), 'Config shape must be (6,)'
if colors is None:
for i in range(len(configs)):
config = configs[i]
new_body = box_body(env, 0.1, 0.05, 0.05, \
name=name + '%d' % i, \
color=(1, 0, 0), \
transparency=transparency)
env.Add(new_body);
set_point(new_body, np.append(config[0:2], 0.075))
new_body.Enable(False)
th = config[2]
set_quat(new_body, quat_from_z_rot(th))
else:
for i in range(len(configs)):
config = configs[i]
if isinstance(colors, tuple):
color = colors
else:
color = colors[i]
new_body = box_body(env, 0.1, 0.05, 0.05, \
name=name + '%d' % i, \
color=color, \
transparency=transparency)
"""
new_body = load_body(env,'mug.xml')
set_name(new_body, name+'%d'%i)
set_transparency(new_body, transparency)
"""
env.Add(new_body);
set_point(new_body, np.append(config[0:2], 0.075))
new_body.Enable(False)
th = config[2]
set_quat(new_body, quat_from_z_rot(th))
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 randomly_place_region(body, region, n_limit=None):
env = openravepy.RaveGetEnvironments()[0]
if env.GetKinBody(get_name(body)) is None:
env.Add(body)
orig = get_body_xytheta(body)
# for _ in n_limit:
i = 0
while True:
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(*range) for range in cspace] +
[region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET])
- aabb.pos() + get_point(body))
if not body_collision(env, body):
return
if n_limit is not None:
i += 1
if i >= n_limit:
set_obj_xytheta(orig, body)
return
def randomly_place_region(env, body, region):
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)
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(env, body): return
def dantam(env): # (Incremental Task and Motion Planning: A Constraint-Based Approach)
assert REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'empty.xml')
set_default_robot_config(env.GetRobots()[0])
set_point(env.GetRobots()[0], (-1.5, 0, 0))
m, n = 3, 3
#m, n = 5, 5
n_obj = 8
side_dim = .07
height_dim = .1
box_dims = (side_dim, side_dim, height_dim)
separation = (side_dim, side_dim)
length = m*(box_dims[0] + separation[0])
width = n*(box_dims[1] + separation[1])
height = .7
table = box_body(env, length, width, height, name='table', color=get_color('tan1'))
set_point(table, (0, 0, 0))
env.Add(table)
pose_indices = list(product(range(m), range(n)))
colors = {}
for r, c in pose_indices:
color = np.zeros(4)
color[2-r] = 1.
colors[(r, c)] = color + float(c)/(n-1)*np.array([1, 0, 0, 0])
poses = {}
z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET
for r, c in pose_indices:
x = get_point(table)[0] - length/2 + (r+.5)*(box_dims[0] + separation[0])
y = get_point(table)[1] - width/2 + (c+.5)*(box_dims[1] + separation[1])
poses[(r, c)] = Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z])))
initial_indices = randomize(pose_indices[:])
initial_poses = {}
goal_poses = {}
for i, indices in enumerate(pose_indices[:n_obj]):
name = 'block%d-%d'%indices
color = colors[indices]
initial_poses[name] = poses[initial_indices[i]]
goal_poses[name] = poses[indices]
obj = box_body(env, *box_dims, name=name, color=color)
set_pose(obj, initial_poses[name].value)
env.Add(obj)
#for obj in randomize(objects):
# randomly_place_body(env, obj, [get_name(table)])
return ManipulationProblem(function_name(inspect.stack()),
object_names=initial_poses.keys(), table_names=[get_name(table)],
goal_poses=goal_poses,
initial_poses=initial_poses, known_poses=poses.values())
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 load_objects(env, obj_shapes, obj_poses, color):
objects = []
i = 0
nobj = len(obj_shapes.keys())
for obj_name in obj_shapes.keys():
xytheta = obj_poses[obj_name].squeeze()
width, length, height = obj_shapes[obj_name]
quat = quat_from_z_rot(xytheta[-1])
new_body = box_body(env, width, length, height, \
name=obj_name, \
color=np.array(color) / float(nobj - i))
i += 1
env.Add(new_body)
set_point(new_body, [xytheta[0], xytheta[1], 0.075])
set_quat(new_body, quat)
objects.append(new_body)
return objects
def load_objects(env, obj_shapes, obj_poses, color):
# sets up the object at their locations in the original env
OBJECTS = []
i = 0
nobj = len(obj_shapes.keys())
for obj_name in obj_shapes.keys():
obj_xyz = obj_poses[obj_name]['obj_xyz']
obj_rot = obj_poses[obj_name]['obj_rot']
width, length, height = obj_shapes[obj_name]
new_body = box_body(env, width, length, height,
name=obj_name,
color=np.array(color) / float(nobj - i))
i += 1
env.Add(new_body)
set_point(new_body, obj_xyz)
set_quat(new_body, obj_rot)
OBJECTS.append(new_body)
return OBJECTS
def grid_arrangement(env, m, n): # (Dealing with Difficult Instances of Object Rearrangment)
assert REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'empty.xml')
box_dims = (.12, .04, .08)
#separation = (.08, .08)
separation = (.12, .12)
#separation = (.16, .16)
length = m*(box_dims[0] + separation[0])
width = n*(box_dims[1] + separation[1])
height = .7
table = box_body(env, length, width, height, name='table', color=get_color('tan1'))
#set_point(table, (1.75, 0, 0))
set_point(table, (0, 0, 0))
env.Add(table)
robot = env.GetRobots()[0]
set_default_robot_config(robot)
set_base_values(robot, (-1.5, 0, 0))
objects = []
goal_poses = {}
z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET
for i in range(m):
x = get_point(table)[0] - length/2 + (i+.5)*(box_dims[0] + separation[0])
row_color = np.zeros(4)
row_color[2-i] = 1.
for j in range(n):
y = get_point(table)[1] - width/2 + (j+.5)*(box_dims[1] + separation[1])
name = 'block%d-%d'%(i, j)
color = row_color + float(j)/(n-1)*np.array([1, 0, 0, 0])
goal_poses[name] = Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z])))
objects.append(box_body(env, *box_dims, name=name, color=color))
object_names = [get_name(body) for body in objects]
for obj in randomize(objects):
randomly_place_body(env, obj, [get_name(table)])
return ManipulationProblem(None,
object_names=object_names, table_names=[get_name(table)],
goal_poses=goal_poses)
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 shelf_arrangement(env): # (Dealing with Difficult Instances of Object Rearrangment)
env.Load(ENVIRONMENTS_DIR + 'empty.xml')
#m, n = 2, 10
m, n = 2, 4
box_dims = (.07, .07, .2)
#separation = (.08, .08)
separation = (.15, .15)
length = m*(box_dims[0] + separation[0])
width = n*(box_dims[1] + separation[1])
height = .7
table = box_body(env, length, width, height, name='table', color=get_color('tan1'))
set_point(table, (1.75, 0, 0))
env.Add(table)
# TODO - place walls and/or a roof to make more similar to pebble graph people
objects = []
goal_poses = {}
z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET
for i in range(m):
x = get_point(table)[0] - length/2 + (i+.5)*(box_dims[0] + separation[0])
row_color = np.zeros(4)
row_color[2-i] = 1.
for j in range(n):
y = get_point(table)[1] - width/2 + (j+.5)*(box_dims[1] + separation[1])
name = 'block%d-%d'%(i, j)
color = row_color + float(j)/(n-1)*np.array([1, 0, 0, 0])
goal_poses[name] = Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z])))
objects.append(box_body(env, *box_dims, name=name, color=color))
object_names = [get_name(body) for body in objects]
for obj in randomize(objects):
randomly_place_body(env, obj, [get_name(table)])
return ManipulationProblem(None,
object_names=object_names, table_names=[get_name(table)],
goal_poses=goal_poses)
def srivastava_table(env):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + '../srivastava/good_cluttered.dae')
camera_trans = camera_look_at((1., -3., 4), look_point=(2, 1, 0))
body_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
dx = .5
for body_name in body_names:
body = env.GetKinBody(body_name)
point = get_point(body)
point[0] += dx
set_point(body, point)
table_names = [
body_name for body_name in body_names if 'table' in body_name
]
object_names = [
body_name for body_name in body_names if body_name not in table_names
]
for object_name in object_names:
body = env.GetKinBody(object_name)
point = get_point(body)
point[2] += BODY_PLACEMENT_Z_OFFSET
set_point(body, point)
goal_holding = 'object1'
goal_config = 'initial' # None
return ManipulationProblem(function_name(inspect.stack()),
camera_trans=camera_trans,
object_names=object_names,
table_names=table_names,
goal_config=goal_config,
goal_holding=goal_holding)
def srivastava_table(env, n=INF):
env.Load(ENVIRONMENTS_DIR + '../srivastava/good_cluttered.dae')
set_default_robot_config(env.GetRobots()[0])
body_names = [get_name(body) for body in env.GetBodies() if not body.IsRobot()]
table_names = [body_name for body_name in body_names if 'table' in body_name]
dx = .5
for body_name in body_names:
body = env.GetKinBody(body_name)
set_point(body, get_point(body) + np.array([dx, 0, 0]))
objects = [env.GetKinBody(body_name) for body_name in body_names if body_name not in table_names]
for obj in objects: env.Remove(obj)
object_names = []
for obj in take(objects, n):
randomly_place_body(env, obj, table_names)
object_names.append(get_name(obj))
goal_holding = 'object1'
goal_config = 'initial' # None
return ManipulationProblem(None,
object_names=object_names, table_names=table_names,
goal_config=goal_config, goal_holding=goal_holding)
def move_several(env, n):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'empty.xml')
box_dims = (.07, .07, .2)
#separation = (.08, .08)
separation = (.10, .10)
length = math.sqrt(n+1)*(box_dims[0] + separation[0])
width = math.sqrt(n+1)*(box_dims[1] + separation[1])
height = .7
table1 = box_body(env, length, width, height, name='table1', color=get_color('tan1'))
set_point(table1, (0, 0, 0))
env.Add(table1)
table2 = box_body(env, length, width, height, name='table2', color=get_color('tan1'))
set_point(table2, (1.5, 0, 0))
env.Add(table2)
robot = env.GetRobots()[0]
set_default_robot_config(robot)
set_base_values(robot, (-1.5, 0, 0))
# TODO - place walls and/or a roof to make more similar to pebble graph people
objects = []
goal_regions = {}
obj = box_body(env, .07, .07, .2, name='blue', color=BLUE)
set_point(obj, (0, 0, height + BODY_PLACEMENT_Z_OFFSET))
objects.append(obj)
goal_regions[get_name(obj)] = get_name(table2)
env.Add(obj)
for i in range(n):
objects.append(box_body(env, .07, .07, .2, name='red'+str(i+1), color=RED))
for obj in randomize(objects[1:]):
randomly_place_body(env, obj, [get_name(table1)])
return ManipulationProblem(None,
object_names=[get_name(body) for body in objects], table_names=[get_name(table) for table in [table1, table2]],
goal_regions=goal_regions)
def sample_placement_using_gen(env,
obj,
robot,
p_samples,
obj_region,
robot_region,
GRAB_SLEEP_TIME=0.05):
# This script, with a given grasp of an object,
# - finding colfree obj placement within 100 tries. no robot at this point
# - checking colfree ik solution for robot; if not, trial is over
# - if above two passes, colfree path finding; if not, trial is over
status = "Failed"
path = None
original_trans = robot.GetTransform()
obj_orig_trans = obj.GetTransform()
tried = []
n_trials = 1 # try 5 different samples of placements with a given grasp
T_r_wrt_o = np.dot(np.linalg.inv(obj.GetTransform()), robot.GetTransform())
for _ in range(n_trials):
#print 'releasing obj'
sleep(GRAB_SLEEP_TIME)
robot.Release(obj)
robot.SetTransform(original_trans)
obj.SetTransform(obj_orig_trans)
#print 'released'
# get robot pose wrt obj
# sample obj pose
#print 'randmly placing obj'
#obj_xytheta = randomly_place_in_region(env,obj,obj_region) # randomly place obj
inCollision = True
np.random.shuffle(p_samples)
for idx, obj_xytheta in enumerate(p_samples):
if idx > 100: break
x = obj_xytheta[0]
y = obj_xytheta[1]
z = get_point(obj)[-1]
set_point(obj, [x, y, z])
th = obj_xytheta[2]
set_quat(obj, quat_from_z_rot(th))
new_T_robot = np.dot(obj.GetTransform(), T_r_wrt_o)
robot.SetTransform(new_T_robot)
inCollision = (check_collision_except(obj,robot,env))\
or (check_collision_except(robot,obj,env))
inRegion = (robot_region.contains(robot.ComputeAABB())) and\
(obj_region.contains(obj.ComputeAABB()))
if (not inCollision) and inRegion:
break
if inCollision or not (inRegion):
break # if you tried all p samples and ran out, get new pick
# compute the resulting robot transform
sleep(GRAB_SLEEP_TIME)
robot.Grab(obj)
robot.SetTransform(new_T_robot)
robot.SetActiveDOFs([],
DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis,
[0, 0, 1])
robot_xytheta = robot.GetActiveDOFValues()
robot.SetTransform(original_trans)
stime = time.time()
for node_lim in [1000, 5000, np.inf]:
path,tpath,status = get_motion_plan(robot,\
robot_xytheta,env,maxiter=10,n_node_lim=node_lim)
if path == 'collision':
# import pdb;pdb.set_trace()
pass
if status == "HasSolution":
robot.SetTransform(new_T_robot)
return obj_xytheta, robot_xytheta, path
else:
print('motion planning failed', tpath)
sleep(GRAB_SLEEP_TIME)
robot.Grab(obj)
robot.SetTransform(original_trans)
print("Returnining no solution")
return None, None, None
def set_obj_xyztheta(xyztheta, obj):
set_point(obj, xyztheta[0:-1])
set_obj_xytheta(np.hstack([xyztheta[0:2], xyztheta[-1]]), obj)
def dantam_distract(env, n_obj): # (Incremental Task and Motion Planning: A Constraint-Based Approach)
assert REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'empty.xml')
m, n = 3, 3
#m, n = 5, 5
side_dim = .07 # .05 | .07
height_dim = .1
box_dims = (side_dim, side_dim, height_dim)
separation = (side_dim, side_dim)
#separation = (side_dim/2, side_dim/2)
coordinates = list(product(range(m), range(n)))
assert n_obj <= len(coordinates)
obj_coordinates = sample(coordinates, n_obj)
length = m*(box_dims[0] + separation[0])
width = n*(box_dims[1] + separation[1])
height = .7
table = box_body(env, length, width, height, name='table', color=get_color('tan1'))
set_point(table, (0, 0, 0))
env.Add(table)
robot = env.GetRobots()[0]
set_default_robot_config(robot)
set_base_values(robot, (-1.5, 0, 0))
#set_base_values(robot, (0, width/2 + .5, math.pi))
#set_base_values(robot, (.35, width/2 + .35, math.pi))
#set_base_values(robot, (.35, width/2 + .35, 3*math.pi/4))
poses = []
z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET
for r in range(m):
row = []
x = get_point(table)[0] - length/2 + (r+.5)*(box_dims[0] + separation[0])
for c in range(n):
y = get_point(table)[1] - width/2 + (c+.5)*(box_dims[1] + separation[1])
row.append(Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z]))))
poses.append(row)
initial_poses = {}
goal_poses = {}
# TODO - randomly assign goal poses here
for i, (r, c) in enumerate(obj_coordinates):
row_color = np.zeros(4)
row_color[2-r] = 1.
if i == 0:
name = 'goal%d-%d'%(r, c)
color = BLUE
goal_poses[name] = poses[m/2][n/2]
else:
name = 'block%d-%d'%(r, c)
color = RED
initial_poses[name] = poses[r][c]
obj = box_body(env, *box_dims, name=name, color=color)
set_pose(obj, poses[r][c].value)
env.Add(obj)
#for obj in randomize(objects):
# randomly_place_body(env, obj, [get_name(table)])
known_poses = list(flatten(poses))
#known_poses = list(set(flatten(poses)) - {poses[r][c] for r, c in obj_coordinates}) # TODO - put the initial poses here
return ManipulationProblem(function_name(inspect.stack()),
object_names=initial_poses.keys(), table_names=[get_name(table)],
goal_poses=goal_poses,
initial_poses=initial_poses, known_poses=known_poses)
