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