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 optimize_angle(robot, link, element_pose, translation, direction, reverse, initial_angles, collision_fn, max_error=1e-2): movable_joints = get_movable_joints(robot) initial_conf = get_joint_positions(robot, movable_joints) best_error, best_angle, best_conf = max_error, None, None for i, angle in enumerate(initial_angles): grasp_pose = get_grasp_pose(translation, direction, angle, reverse) target_pose = multiply(element_pose, invert(grasp_pose)) conf = inverse_kinematics(robot, link, target_pose) # if conf is None: # continue #if pairwise_collision(robot, robot): conf = get_joint_positions(robot, movable_joints) if not collision_fn(conf): link_pose = get_link_pose(robot, link) error = get_distance(point_from_pose(target_pose), point_from_pose(link_pose)) if error < best_error: # TODO: error a function of direction as well best_error, best_angle, best_conf = error, angle, conf # wait_for_interrupt() if i != len(initial_angles) - 1: set_joint_positions(robot, movable_joints, initial_conf) #print(best_error, translation, direction, best_angle) if best_conf is not None: set_joint_positions(robot, movable_joints, best_conf) #wait_for_interrupt() return best_angle, best_conf
def test_grasps(robot, node_points, elements): element = elements[-1] [element_body] = create_elements(node_points, [element]) phi = 0 link = link_from_name(robot, TOOL_NAME) link_pose = get_link_pose(robot, link) draw_pose(link_pose) #, parent=robot, parent_link=link) wait_for_interrupt() n1, n2 = element p1 = node_points[n1] p2 = node_points[n2] length = np.linalg.norm(p2 - p1) # Bottom of cylinder is p1, top is p2 print(element, p1, p2) for phi in np.linspace(0, np.pi, 10, endpoint=True): theta = np.pi / 4 for t in np.linspace(-length / 2, length / 2, 10): translation = Pose( point=Point(z=-t) ) # Want to be moving backwards because +z is out end effector direction = Pose(euler=Euler(roll=np.pi / 2 + theta, pitch=phi)) angle = Pose(euler=Euler(yaw=1.2)) grasp_pose = multiply(angle, direction, translation) element_pose = multiply(link_pose, grasp_pose) set_pose(element_body, element_pose) wait_for_interrupt()
def apply(self, state, **kwargs): # TODO: identify surface automatically with LockRenderer(): cone = get_viewcone(color=(1, 0, 0, 0.5)) set_pose(cone, get_link_pose(self.robot, self.link)) wait_for_duration(1e-2) wait_for_duration(self._duration) # TODO: don't sleep if no viewer? remove_body(cone) wait_for_duration(1e-2) if self.detect: # TODO: the collision geometries are being visualized # TODO: free the renderer detections = get_visual_detections(self.robot, camera_link=self.camera_frame) print('Detections:', detections) for body, dist in state.b_on.items(): obs = (body in detections) and (is_center_stable( body, self.surface)) if obs or (self.surface not in state.task.rooms): # TODO: make a command for scanning a room instead? dist.obsUpdate(get_observation_fn(self.surface), obs) #state.localized.update(detections) # TODO: pose for each object that can be real or fake yield
def fn(robot, conf, body, grasp): conf.assign() link = link_from_name(robot, BASE_LINK) world_from_body = multiply(get_link_pose(robot, link), grasp.value) pose = Pose(body, world_from_body) pose.assign() if any(pairwise_collision(body, obst) for obst in problem.obstacles): return None return (pose, )
def gen(rover, objective): base_joints = get_base_joints(rover) target_point = get_point(objective) base_generator = visible_base_generator(rover, target_point, base_range) lower_limits, upper_limits = get_custom_limits(rover, base_joints, custom_limits) attempts = 0 while True: if max_attempts <= attempts: attempts = 0 yield None attempts += 1 base_conf = next(base_generator) if not all_between(lower_limits, base_conf, upper_limits): continue bq = Conf(rover, base_joints, base_conf) bq.assign() if any(pairwise_collision(rover, b) for b in obstacles): continue link_pose = get_link_pose(rover, link_from_name(rover, KINECT_FRAME)) if use_cone: mesh, _ = get_detection_cone(rover, objective, camera_link=KINECT_FRAME, depth=max_range) if mesh is None: continue cone = create_mesh(mesh, color=(0, 1, 0, 0.5)) local_pose = Pose() else: distance = get_distance(point_from_pose(link_pose), target_point) if max_range < distance: continue cone = create_cylinder(radius=0.01, height=distance, color=(0, 0, 1, 0.5)) local_pose = Pose(Point(z=distance / 2.)) set_pose(cone, multiply(link_pose, local_pose)) # TODO: colors corresponding to scanned object if any( pairwise_collision(cone, b) for b in obstacles if b != objective and not is_placement(objective, b)): # TODO: ensure that this works for the rover remove_body(cone) continue if not reachable_test(rover, bq): continue print('Visibility attempts:', attempts) y = Ray(cone, rover, objective) yield Output(bq, y)
def apply(self, state, **kwargs): mesh, _ = get_detection_cone(self.robot, self.body, depth=MAX_KINECT_DISTANCE) assert (mesh is not None) cone = create_mesh(mesh, color=(0, 1, 0, 0.5)) set_pose(cone, get_link_pose(self.robot, self.link)) wait_for_duration(self._duration) # time.sleep(1.0) remove_body(cone) state.registered.add(self.body)
def apply(self, state, **kwargs): # TODO: identify surface automatically cone = get_viewcone(color=(1, 0, 0, 0.5)) set_pose(cone, get_link_pose(self.robot, self.link)) wait_for_duration(self._duration) # TODO: don't sleep if no viewer? remove_body(cone) detections = get_visual_detections(self.robot) print('Detections:', detections) for body, dist in state.b_on.items(): obs = (body in detections) and (is_center_stable( body, self.surface)) if obs or (self.surface not in state.task.rooms): # TODO: make a command for scanning a room instead? dist.obsUpdate(get_observation_fn(self.surface), obs)
def test_ik(robot, node_order, node_points): link = link_from_name(robot, TOOL_NAME) movable_joints = get_movable_joints(robot) sample_fn = get_sample_fn(robot, movable_joints) for n in node_order: point = node_points[n] set_joint_positions(robot, movable_joints, sample_fn()) conf = inverse_kinematics(robot, link, (point, None)) if conf is not None: set_joint_positions(robot, movable_joints, conf) continue link_point, link_quat = get_link_pose(robot, link) #print(point, link_point) #user_input('Continue?') wait_for_interrupt()
def apply(self, state, **kwargs): mesh, _ = get_detection_cone(self.robot, self.body, camera_link=self.camera_frame, depth=self.max_depth) if mesh is None: wait_for_user() assert (mesh is not None) with LockRenderer(): cone = create_mesh(mesh, color=(0, 1, 0, 0.5)) set_pose(cone, get_link_pose(self.robot, self.link)) wait_for_duration(1e-2) wait_for_duration(self._duration) remove_body(cone) wait_for_duration(1e-2) state.registered.add(self.body) yield
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, )
def apply(self, state, **kwargs): # TODO: check if actually can register mesh, _ = get_detection_cone(self.robot, self.body, camera_link=self.camera_frame, depth=self.max_depth) if mesh is None: wait_for_user() assert ( mesh is not None ) # TODO: is_visible_aabb(body_aabb, **kwargs)=False sometimes without collisions with LockRenderer(): cone = create_mesh(mesh, color=apply_alpha(GREEN, 0.5)) set_pose(cone, get_link_pose(self.robot, self.link)) wait_for_duration(1e-2) wait_for_duration(self._duration) remove_body(cone) wait_for_duration(1e-2) state.registered.add(self.body) yield
def display_trajectories(ground_nodes, trajectories, time_step=0.05): if trajectories is None: return connect(use_gui=True) floor, robot = load_world() wait_for_interrupt() movable_joints = get_movable_joints(robot) #element_bodies = dict(zip(elements, create_elements(node_points, elements))) #for body in element_bodies.values(): # set_color(body, (1, 0, 0, 0)) connected = set(ground_nodes) for trajectory in trajectories: if isinstance(trajectory, PrintTrajectory): print(trajectory, trajectory.n1 in connected, trajectory.n2 in connected, is_ground(trajectory.element, ground_nodes), len(trajectory.path)) connected.add(trajectory.n2) #wait_for_interrupt() #set_color(element_bodies[element], (1, 0, 0, 1)) last_point = None handles = [] for conf in trajectory.path: set_joint_positions(robot, movable_joints, conf) if isinstance(trajectory, PrintTrajectory): current_point = point_from_pose( get_link_pose(robot, link_from_name(robot, TOOL_NAME))) if last_point is not None: color = (0, 0, 1) if is_ground(trajectory.element, ground_nodes) else (1, 0, 0) handles.append( add_line(last_point, current_point, color=color)) last_point = current_point wait_for_duration(time_step) #wait_for_interrupt() #user_input('Finish?') wait_for_interrupt() disconnect()