def gen(robot, body): link = link_from_name(robot, BASE_LINK) with BodySaver(robot): set_base_conf(robot, np.zeros(3)) robot_pose = get_link_pose(robot, link) robot_aabb = get_turtle_aabb(robot) # _, upper = robot_aabb # radius = upper[0] extent = get_aabb_extent(robot_aabb) radius = max(extent[:2]) / 2. #draw_aabb(robot_aabb) center, (diameter, height) = approximate_as_cylinder(body) distance = radius + diameter / 2. + epsilon _, _, z = get_point(body) # Assuming already placed stably for [scale] in unit_generator(d=1, use_halton=use_halton): #theta = PI # 0 | PI theta = random.uniform(*theta_interval) position = np.append(distance * unit_from_theta(theta=theta), [z]) # TODO: halton yaw = scale * 2 * PI - PI quat = quat_from_euler(Euler(yaw=yaw)) body_pose = (position, quat) robot_from_body = multiply(invert(robot_pose), body_pose) grasp = Pose(body, robot_from_body) # TODO: grasp instead of pose if draw: world_pose = multiply(get_link_pose(robot, link), grasp.value) set_pose(body, world_pose) handles = draw_pose(get_pose(body), length=1) wait_for_user() remove_handles(handles) #continue yield (grasp, )
def to_points(self, link=BASE_LINK): points = [] for conf in self.path: with BodySaver(conf.body): conf.step() #point = np.array(point_from_pose(get_link_pose(conf.body, link))) point = np.array(get_group_conf(conf.body, 'base')) point[2] = 0 point += 1e-2 * np.array([0, 0, 1]) if not (points and np.allclose(points[-1], point, atol=1e-3, rtol=0)): points.append(point) points = get_target_path(self) return waypoints_from_path(points)
def get_target_point(conf): # TODO: use full body aabb robot = conf.body link = link_from_name(robot, 'torso_lift_link') #link = BASE_LINK # TODO: center of mass instead? # TODO: look such that cone bottom touches at bottom # TODO: the target isn't the center which causes it to drift with BodySaver(conf.body): conf.step() lower, upper = get_aabb(robot, link) center = np.average([lower, upper], axis=0) point = np.array(get_group_conf(conf.body, 'base')) #point[2] = upper[2] point[2] = center[2] #center, _ = get_center_extent(conf.body) return point
def fn(bq1, bq2): if teleport: path = [bq1, bq2] elif not is_drake_pr2(robot): set_pose(robot, bq1.value) #start_conf = base_values_from_pose(bq1.value) goal_conf = base_values_from_pose(bq2.value) raw_path = plan_base_motion(robot, goal_conf, BASE_LIMITS, obstacles=fixed) if raw_path is None: print('Failed motion plan!') return None path = [ Pose(robot, pose_from_base_values(q, bq1.value)) for q in raw_path ] else: base_joints = get_group_joints(robot, 'base') if PLAN_JOINTS: # set_pose(robot, unit_pose()) # set_group_conf(robot, 'base', start_conf) set_joint_positions(robot, base_joints, bq1.values) raw_path = plan_joint_motion(robot, base_joints, bq2.values, obstacles=fixed, self_collisions=False) else: with BodySaver(robot): set_base_values(robot, bq1.values) set_joint_positions(robot, base_joints, np.zeros(len(base_joints))) raw_path = plan_base_motion(robot, bq2.values, BASE_LIMITS, obstacles=fixed) if raw_path is None: print('Failed motion plan!') return None #path = [Pose(robot, pose_from_base_values(q, bq1.value)) for q in raw_path] path = [Conf(robot, base_joints, q) for q in raw_path] bt = Trajectory(path) return (bt, )
def move_look_trajectory(task, trajectory, max_tilt=np.pi / 6): # max_tilt=INF): # TODO: implement a minimum distance instead of max_tilt # TODO: pr2 movement restrictions #base_path = [pose.to_base_conf() for pose in trajectory.path] base_path = trajectory.path if not base_path: return trajectory obstacles = get_fixed_bodies(task) # TODO: movable objects robot = base_path[0].body target_path = get_target_path(trajectory) waypoints = [] index = 0 with BodySaver(robot): #current_conf = base_values_from_pose(get_pose(robot)) for i, conf in enumerate(base_path): # TODO: just do two loops? conf.assign() while index < len(target_path): if i < index: # Don't look at past or current conf target_point = target_path[index] head_conf = inverse_visibility( robot, target_point) # TODO: this is slightly slow #print(index, target_point, head_conf) if (head_conf is not None) and (head_conf[1] < max_tilt): break index += 1 else: head_conf = get_group_conf(robot, 'head') set_group_conf(robot, 'head', head_conf) #print(i, index, conf.values, head_conf) #, get_pose(robot)) waypoints.append(np.concatenate([conf.values, head_conf])) joints = tuple(base_path[0].joints) + tuple(get_group_joints( robot, 'head')) #joints = get_group_joints(robot, 'base') + get_group_joints(robot, 'head') #set_pose(robot, unit_pose()) #set_group_conf(robot, 'base', current_conf) path = plan_waypoints_joint_motion(robot, joints, waypoints, obstacles=obstacles, self_collisions=SELF_COLLISIONS) return create_trajectory(robot, joints, path)
def gen(robot, body): link = link_from_name(robot, BASE_LINK) with BodySaver(robot): set_base_conf(robot, np.zeros(3)) robot_pose = get_link_pose(robot, link) robot_aabb = get_turtle_aabb(robot) #draw_aabb(robot_aabb) lower, upper = robot_aabb center, (diameter, height) = approximate_as_cylinder(body) _, _, z = get_point(body) # Assuming already placed stably position = Point(x=upper[0] + diameter / 2., z=z) for [scale] in halton_generator(d=1): yaw = scale * 2 * np.pi - np.pi quat = quat_from_euler(Euler(yaw=yaw)) body_pose = (position, quat) robot_from_body = multiply(invert(robot_pose), body_pose) grasp = Pose(body, robot_from_body) yield (grasp, )