def look_at(robot, body):
manipulator = robot.GetManipulator(HEAD_NAME)
head_point = get_point(manipulator)
body_point = get_point(body) # Use center of mass, bbox center, etc...
dx, dy, dz = body_point - head_point
theta = atan2(dy, dx)
phi = -atan2(dz, sqrt(dx**2 + dy**2))
solution = np.array([theta, phi]) # TODO - check if within limits
robot.SetDOFValues(solution, manipulator.GetArmIndices())
return solution
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 cylinder_collision(oracle, body_name1, body_name2):
# TODO
# - ODE cylinder-cylinder collision doesn't seem to work reliably. But PQP does work. Set the PQP just for cylinders
# - Incorporate cylinder1.GetTransform() for local point
# - Just use the aabb
# - Only considers case where cylinders are resting on their base
body1, body2 = oracle.get_body(body_name1), oracle.get_body(body_name2)
cylinder1, cylinder2 = oracle.get_geometries(
body_name1)[0], oracle.get_geometries(body_name2)[0]
if is_upright(get_trans(body1)) and is_upright(get_trans(body2)):
point1, point2 = get_point(body1), get_point(body2)
return abs(point1[2] - point2[2]) < (cylinder1.GetCylinderHeight() + cylinder2.GetCylinderHeight())/2. and \
length(point1[:2] - point2[:2]) < cylinder1.GetCylinderRadius() + cylinder2.GetCylinderRadius()
return oracle.env.CheckCollision(body1, body2)
def sample_ir_multiple_regions(obj, robot, env, multiple_regions):
arm_len = 0.9844 # determined by spreading out the arm and measuring the dist from shoulder to ee
# grasp_pos = Tgrasp[0:-1,3]
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 sample_pick_using_gen(obj, obj_shape, robot, generator, env, region):
# diable all bodies; imagine being able to pick anywhere
for body in env.GetBodies():
body.Enable(False)
# enable the target and the robot
obj.Enable(True)
robot.Enable(True)
original_trans = robot.GetTransform()
n_trials = 100 # try 20 different samples
for idx in range(n_trials):
theta, height_portion, depth_portion, base_pose \
= generate_pick_grasp_and_base_pose(generator, obj_shape, get_point(obj))
set_robot_config(base_pose, robot)
grasps = compute_two_arm_grasp(depth_portion, height_portion, theta, obj, robot) # tool trans
g_config = solveTwoArmIKs(env, robot, obj, grasps) # turns obj collision off
if g_config is None:
continue
for body in env.GetBodies():
if body.GetName().find('_pt_') != -1: continue
body.Enable(True)
return base_pose, [theta, height_portion, depth_portion], g_config
return None, None, None
def sample_ir(obj, robot, env, region, n_iter=300):
arm_len = PR2_ARM_LENGTH # determined by spreading out the arm and measuring the dist from shoulder to ee
# grasp_pos = Tgrasp[0:-1,3]
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(n_iter):
robot_xy = sample_xy_locations(obj, arm_len)[:-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 (region.contains(robot.ComputeAABB())):
return np.array([robot_xy[0], robot_xy[1], angle])
return None
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 simple_push(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', 'table2']
place_body(env,
cylinder_body(env, .07, .05, name='blue_cylinder', color=BLUE),
(1.65, -.6, PI / 4), 'table1')
object_names = [
str(body.GetName()) for body in env.GetBodies()
if not body.IsRobot() and str(body.GetName()) not in obstacle_names
]
start_constrained = {'blue_cylinder': 'table1'}
goal_constrained = {
'blue_cylinder': ('table2',
Point(
get_point(env.GetKinBody('blue_cylinder')) +
np.array([-1.65, -1., 0.])))
#'blue_cylinder': ('table1', Point(get_point(env.GetKinBody('blue_cylinder')) + np.array([-.2,.8, 0.])))
}
return ManipulationProblem(function_name(inspect.stack()),
object_names=object_names,
table_names=table_names,
start_constrained=start_constrained,
goal_constrained=goal_constrained)
def trans_from_xytheta(obj, xytheta):
rot = rot_from_quat(quat_from_z_rot(xytheta[-1]))
z = get_point(obj)[-1]
trans = np.eye(4)
trans[:3, :3] = rot
trans[:3, -1] = [xytheta[0], xytheta[1], z]
return trans
def get_body_xytheta(body):
if not isinstance(body, openravepy.KinBody):
env = openravepy.RaveGetEnvironments()[0]
body = env.GetKinBody(body)
Tbefore = body.GetTransform()
body_quat = get_quat(body)
th1 = np.arccos(body_quat[0]) * 2
th2 = np.arccos(-body_quat[0]) * 2
th3 = -np.arccos(body_quat[0]) * 2
quat_th1 = quat_from_angle_vector(th1, np.array([0, 0, 1]))
quat_th2 = quat_from_angle_vector(th2, np.array([0, 0, 1]))
quat_th3 = quat_from_angle_vector(th3, np.array([0, 0, 1]))
if np.all(np.isclose(body_quat, quat_th1)):
th = th1
elif np.all(np.isclose(body_quat, quat_th2)):
th = th2
elif np.all(np.isclose(body_quat, quat_th3)):
th = th3
else:
print "This should not happen. Check if object is not standing still"
import pdb;
pdb.set_trace()
if th < 0: th += 2 * np.pi
assert (0 <= th < 2 * np.pi)
# set the quaternion using the one found
set_quat(body, quat_from_angle_vector(th, np.array([0, 0, 1])))
Tafter = body.GetTransform()
assert (np.all(np.isclose(Tbefore, Tafter)))
body_xytheta = np.hstack([get_point(body)[0:2], th])
body_xytheta = body_xytheta[None, :]
clean_pose_data(body_xytheta)
return body_xytheta
def __init__(self, env):
objects_in_env = env.GetBodies()
self.env_id = 1
self.robot_name = 'pr2'
robot = env.GetRobot(self.robot_name)
self.object_poses = {
o.GetName(): get_body_xytheta(o)
for o in objects_in_env
}
self.object_poses = {}
for o in objects_in_env:
xyz = get_point(o)
xytheta = get_body_xytheta(o)
xyztheta = np.hstack([xyz, xytheta.squeeze()[-1]])
self.object_poses[o.GetName()] = xyztheta
# todo set xyztheta
self.robot_base_pose = get_body_xytheta(robot)
self.robot_dof_values = robot.GetDOFValues()
self.is_holding = len(robot.GetGrabbed()) > 0
if self.is_holding:
self.held_object = robot.GetGrabbed()[0].GetName()
else:
self.held_object = 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 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 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_region(body, region):
env = openravepy.RaveGetEnvironments()[0]
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 look_at_ik(oracle, body_name):
robot = oracle.robot
manipulator = robot.GetManipulator(HEAD_NAME)
body_point = get_point(
oracle.bodies[body_name]) # Use center of mass, bbox center, etc...
ik_param = IkParameterization(body_point, IkParameterization.Type.Lookat3D)
solutions = oracle.look_model.manip.FindIKSolutions(
ik_param, IkFilterOptions.CheckEnvCollisions)
if len(solutions) == 0:
return None
assert len(solutions) == 1
solution = solutions[0]
robot.SetDOFValues(solution, manipulator.GetArmIndices())
return solution
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 push_wall(env):
assert not REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'room1.xml')
camera_trans = camera_look_at(CAMERA_POINT_SCALE * np.array([-1, 1, 3]),
look_point=(3, 0, 0))
obstacle_names = [
str(body.GetName()) for body in env.GetBodies() if not body.IsRobot()
]
table_names = ['table1', 'table2']
place_body(
env, cylinder_body(env, .07, .05, name='green_cylinder', color=GREEN),
(1.65, -.5, PI / 4), 'table1')
#place_body(env, box_body(env, .07, .05, .2, name='green_box', color=GREEN), (1.65, -.6, 0), 'table1')
#set_point(env.GetKinBody('green_box'), get_point(env.GetKinBody('green_box')) + .01*unit_z())
place_body(env, box_body(env, .05, .05, .15, name='red_box1', color=RED),
(1.50, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box2', color=RED),
(1.6, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box3', color=RED),
(1.7, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box4', color=RED),
(1.8, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box5', color=RED),
(1.9, 0, 0), 'table1')
place_body(env, box_body(env, .05, .05, .15, name='red_box6', color=RED),
(2.0, 0, 0), 'table1')
object_names = [
str(body.GetName()) for body in env.GetBodies()
if not body.IsRobot() and str(body.GetName()) not in obstacle_names
]
start_constrained = {'green_cylinder': 'table1'}
goal_constrained = {
'green_cylinder': ('table1',
Point(
get_point(env.GetKinBody('green_cylinder')) +
np.array([-.2, 1.0, 0.])))
}
return ManipulationProblem(function_name(inspect.stack()),
camera_trans=camera_trans,
object_names=object_names,
table_names=table_names,
start_constrained=start_constrained,
goal_constrained=goal_constrained)
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 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 solveTwoArmIKs(env, robot, obj, grasps):
leftarm_manip = robot.GetManipulator('leftarm')
rightarm_manip = robot.GetManipulator('rightarm')
rightarm_torso_manip = robot.GetManipulator('rightarm_torso')
arm_len = 0.9844 # determined by spreading the arm and measuring the dist from shoulder to ee
# a grasp consists of:
# g[0] = left grasp
# g[1] = right grasp
# g[2] = grasp wrt obj; used to filter out robot poses that are not facing the same direction
for g in grasps:
# first check if g within reach
g_left = g[0]
g_right = g[1]
yaw_wrt_obj = g[2]
Tleft_ee = compute_Tee_at_given_Ttool(g_left, \
leftarm_manip.GetLocalToolTransform())
Tright_ee = compute_Tee_at_given_Ttool(g_right, \
rightarm_manip.GetLocalToolTransform())
left_ee_xy = point_from_trans(Tleft_ee)[:-1]
right_ee_xy = point_from_trans(Tright_ee)[:-1]
mid_grasp_xy = (left_ee_xy + right_ee_xy) / 2
robot_xy = get_point(robot)[:-1]
obj_xy = get_point(obj)[:-1]
# filter the trivial conditions IK wont be found
# condition 1:
# robot and the grasp must face the same direction
# where is robot facing?
r_quat = get_quat(robot)
o_quat = get_quat(obj)
# refer to wiki on Conversion between Euler and Quaternion for these eqns
r_z_rot = np.arccos(r_quat[0]) * 2 if r_quat[-1] >= 0 else np.arccos(-r_quat[0]) * 2
o_z_rot = np.arccos(o_quat[0]) * 2 if o_quat[-1] >= 0 else np.arccos(-o_quat[0]) * 2
r_z_rot *= 180 / PI;
o_z_rot *= 180 / PI
if r_z_rot < 0: r_z_rot + 360
if o_z_rot < 0: o_z_rot + 360
angle_diff = r_z_rot - o_z_rot
if angle_diff < 0: angle_diff += 360
# ugh, look at the notes to figure out how I did the thing below
if (angle_diff < 45 or (angle_diff <= 360 and angle_diff >= 315) and (yaw_wrt_obj != PI / 2)) \
or (angle_diff < 135 and angle_diff >= 45 and yaw_wrt_obj != PI) \
or (angle_diff < 225 and angle_diff >= 135 and yaw_wrt_obj != 3 * PI / 2) \
or (angle_diff < 310 and angle_diff >= 225 and yaw_wrt_obj != 0):
continue
# condition 2: ee loc must be within reach
right_ee_dist = np.linalg.norm(robot_xy - right_ee_xy)
left_ee_dist = np.linalg.norm(robot_xy - left_ee_xy)
if right_ee_dist > arm_len * 0.75 or left_ee_dist > arm_len * 0.75:
continue
# checking right arm ik solution feasibility
obj.Enable(False)
right_g_config = rightarm_torso_manip.FindIKSolution(g_right, 0)
# rightarm_torso_manip.GetEndEffector().SetTransform(Tright_ee)
if right_g_config is None:
obj.Enable(True)
continue
# turning checkenvcollision option for FindIKSolution seems take excessive amt of time
with robot:
obj.Enable(True)
set_config(robot, right_g_config, rightarm_torso_manip.GetArmIndices())
if env.CheckCollision(robot, obj):
right_g_config = None
# checking left arm ik solution feasibility
st = time.time()
obj.Enable(False)
left_g_config = leftarm_manip.FindIKSolution(g_left, 0)
with robot:
obj.Enable(True)
set_config(robot, left_g_config, leftarm_manip.GetArmIndices())
if env.CheckCollision(robot, obj):
left_g_config = None
if left_g_config is None:
obj.Enable(True)
continue
obj.Enable(True)
if not (left_g_config is None) and not (right_g_config is None):
obj.Enable(True)
return [left_g_config, right_g_config]
return None
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
length = 5
height = 4
objB = box_body(env, width, length, height, \
name='objB', \
color=(1, 1, 1))
env.Add(objB)
set_point(objB, [1, 0, 0])
posB = objB.ComputeAABB().pos()
extB = objB.ComputeAABB().extents()
k = 1
vtx = box_body(env, 0.1, 0.1, 0.1, name='vtx+%d' % (k), color=(1, 0, 0))
env.Add(vtx)
set_point(vtx, posB + extB)
# vtx rel to objB?
vtx_wrt_B = np.linalg.solve(get_trans(objB),
np.concatenate([get_point(vtx),
np.array([1])]))
set_quat(objB, quat_from_z_rot(PI / 2))
new_point = translate_point(get_trans(objB), vtx_wrt_B)
set_point(vtx, new_point)
import pdb
pdb.set_trace()
### Notations
# w = world
# o = obj
# ee = end-effector
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)
width = 3
length = 5
height = 4
objB = box_body(env,width,length,height,\
name='objB',\
color=(1, 1, 1))
env.Add(objB)
set_point(objB,[1,0,0])
posB = objB.ComputeAABB().pos()
extB = objB.ComputeAABB().extents()
k=1
vtx = box_body(env,0.1,0.1,0.1,name='vtx+%d'%(k),color=(1,0,0))
env.Add(vtx)
set_point(vtx,posB+extB)
# vtx rel to objB?
vtx_wrt_B = np.linalg.solve(get_trans(objB),np.concatenate([get_point(vtx), np.array([1])]))
set_quat(objB,quat_from_z_rot(PI/2))
new_point= translate_point(get_trans(objB),vtx_wrt_B)
set_point(vtx,new_point)
import pdb;pdb.set_trace()
### Notations
# w = world
# o = obj
# ee = end-effector
def compute_tool_trans_wrt_obj_trans(tool_trans_wrt_world,object_trans):
return np.linalg.solve(object_trans, tool_trans_wrt_world)
def compute_Tee_at_given_Ttool( tool_trans_wrt_world,tool_trans_wrt_ee ):
