def test_retraction(robot, info, tool_link, distance=0.1, **kwargs): ik_joints = get_ik_joints(robot, info, tool_link) start_pose = get_link_pose(robot, tool_link) end_pose = multiply(start_pose, Pose(Point(z=-distance))) handles = [ add_line(point_from_pose(start_pose), point_from_pose(end_pose), color=BLUE) ] #handles.extend(draw_pose(start_pose)) #handles.extend(draw_pose(end_pose)) path = [] pose_path = list( interpolate_poses(start_pose, end_pose, pos_step_size=0.01)) for i, pose in enumerate(pose_path): print('Waypoint: {}/{}'.format(i + 1, len(pose_path))) handles.extend(draw_pose(pose)) conf = next( either_inverse_kinematics(robot, info, tool_link, pose, **kwargs), None) if conf is None: print('Failure!') path = None wait_for_user() break set_joint_positions(robot, ik_joints, conf) path.append(conf) wait_for_user() # for conf in islice(ikfast_inverse_kinematics(robot, info, tool_link, pose, max_attempts=INF, max_distance=0.5), 1): # set_joint_positions(robot, joints[:len(conf)], conf) # wait_for_user() remove_handles(handles) return path
def are_visible(world): ray_names = [] rays = [] for name in world.movable: for camera, info in world.cameras.items(): camera_pose = get_pose(info.body) camera_point = point_from_pose(camera_pose) point = point_from_pose(get_pose(world.get_body(name))) if is_visible_point(CAMERA_MATRIX, KINECT_DEPTH, point, camera_pose=camera_pose): ray_names.append(name) rays.append(Ray(camera_point, point)) ray_results = batch_ray_collision(rays) visible_indices = [ idx for idx, (name, result) in enumerate(zip(ray_names, ray_results)) if result.objectUniqueId == world.get_body(name) ] visible_names = {ray_names[idx] for idx in visible_indices} print('Detected:', sorted(visible_names)) if has_gui(): handles = [ add_line(rays[idx].start, rays[idx].end, color=BLUE) for idx in visible_indices ] wait_for_duration(1.0) remove_handles(handles) # TODO: the object drop seems to happen around here return visible_names
def main(): # The URDF loader seems robust to package:// and slightly wrong relative paths? connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") with LockRenderer(): with HideOutput(): robot = load_model(MOVO_URDF, fixed_base=True) for link in get_links(robot): set_color(robot, color=apply_alpha(0.25 * np.ones(3), 1), link=link) base_joints = joints_from_names(robot, BASE_JOINTS) draw_base_limits((get_min_limits( robot, base_joints), get_max_limits(robot, base_joints)), z=1e-2) dump_body(robot) wait_for_user('Start?') #for arm in ARMS: # test_close_gripper(robot, arm) arm = 'right' tool_link = link_from_name(robot, TOOL_LINK.format(arm)) #joint_names = HEAD_JOINTS #joints = joints_from_names(robot, joint_names) joints = base_joints + get_arm_joints(robot, arm) #joints = get_movable_joints(robot) print('Joints:', get_joint_names(robot, joints)) ik_joints = get_ik_joints(robot, MOVO_INFOS[arm], tool_link) #fixed_joints = [] fixed_joints = ik_joints[:1] #fixed_joints = ik_joints sample_fn = get_sample_fn(robot, joints) handles = [] for i in range(10): print('Iteration:', i) conf = sample_fn() print(conf) set_joint_positions(robot, joints, conf) tool_pose = get_link_pose(robot, tool_link) remove_handles(handles) handles = draw_pose(tool_pose) wait_for_user() #conf = next(ikfast_inverse_kinematics(robot, MOVO_INFOS[arm], tool_link, tool_pose, # fixed_joints=fixed_joints, max_time=0.1), None) #if conf is not None: # set_joint_positions(robot, ik_joints, conf) #wait_for_user() test_retraction(robot, MOVO_INFOS[arm], tool_link, fixed_joints=fixed_joints, max_time=0.1) disconnect()
def draw(self, **kwargs): with LockRenderer(True): remove_handles(self.handles) self.handles = [] with WorldSaver(): for name, pose_dist in self.pose_dists.items(): self.handles.extend( pose_dist.draw(color=self.color_from_name[name], **kwargs))
def follow_curve(robot, joints, curve, goal_t=None, time_step=None, max_velocities=MAX_VELOCITIES, **kwargs): if goal_t is None: goal_t = curve.x[-1] if time_step is None: time_step = 10 * get_time_step() #distance_fn = get_distance_fn(robot, joints, weights=None, norm=2) distance_fn = get_duration_fn(robot, joints, velocities=max_velocities, norm=INF) # get_distance positions = np.array(get_joint_positions(robot, joints)) closest_dist, closest_t = find_closest(positions, curve, t_range=(curve.x[0], goal_t), max_time=1e-1, max_iterations=INF, distance_fn=distance_fn, verbose=True) print('Closest dist: {:.3f} | Closest time: {:.3f}'.format( closest_dist, closest_t)) target_t = closest_t # TODO: condition based on closest_dist while True: print('\nTarget time: {:.3f} | Goal time: {}'.format(target_t, goal_t)) target_positions = curve(target_t) target_velocities = curve(target_t, nu=1) # TODO: draw the velocity #print('Positions: {} | Velocities: {}'.format(target_positions, target_velocities)) handles = draw_waypoint(target_positions) is_goal = (target_t == goal_t) position_tol = 1e-2 if is_goal else 1e-2 for output in control_state(robot, joints, target_positions=target_positions, target_velocities=target_velocities, position_tol=position_tol, velocity_tol=INF, max_velocities=max_velocities, **kwargs): yield output remove_handles(handles) target_t = min(goal_t, target_t + time_step) if is_goal: break
def visualize_learned_pours(learner, bowl_type='blue_bowl', cup_type='red_cup', num_steps=None): learner.query_type = REJECTION # BEST | REJECTION | STRADDLE learner.validity_learner = None world = create_world() #add_obstacles() #environment = get_bodies() world.bodies[bowl_type] = load_cup_bowl(bowl_type) world.bodies[cup_type] = load_cup_bowl(cup_type) feature = get_pour_feature(world, bowl_type, cup_type) draw_pose(get_pose(world.get_body(bowl_type)), length=0.2) # TODO: sample different sizes print('Feature:', feature) for parameter in learner.parameter_generator(world, feature, valid=False): handles = [] print('Parameter:', parameter) valid = is_valid_pour(world, feature, parameter) score = learner.score(feature, parameter) x = learner.func.x_from_feature_parameter(feature, parameter) [prediction] = learner.predict_x(np.array([x]), noise=True) print('Valid: {} | Mean: {:.3f} | Var: {:.3f} | Score: {:.3f}'.format( valid, prediction['mean'], prediction['variance'], score)) #fraction = rescale(clip(prediction['mean'], *DEFAULT_INTERVAL), DEFAULT_INTERVAL, new_interval=(0, 1)) #color = (1 - fraction) * np.array(RED) + fraction * np.array(GREEN) #set_color(world.get_body(cup_type), apply_alpha(color, alpha=0.25)) # TODO: overlay many cups at different poses bowl_from_pivot = get_bowl_from_pivot(world, feature, parameter) #handles.extend(draw_pose(bowl_from_pivot)) handles.extend(draw_point(point_from_pose(bowl_from_pivot), color=BLACK)) # TODO: could label bowl, cup dimensions path, _ = pour_path_from_parameter(world, feature, parameter, cup_yaw=0) for p1, p2 in safe_zip(path[:-1], path[1:]): handles.append(add_line(point_from_pose(p1), point_from_pose(p2), color=RED)) if num_steps is not None: path = path[:num_steps] for i, pose in enumerate(path[::-1]): #step_handles = draw_pose(pose, length=0.1) #step_handles = draw_point(point_from_pose(pose), color=BLACK) set_pose(world.get_body(cup_type), pose) print('Step {}/{}'.format(i+1, len(path))) wait_for_user() #remove_handles(step_handles) remove_handles(handles)
def test_grasps(robot, block): for arm in ARMS: gripper_joints = get_gripper_joints(robot, arm) tool_link = link_from_name(robot, TOOL_LINK.format(arm)) tool_pose = get_link_pose(robot, tool_link) #handles = draw_pose(tool_pose) #grasps = get_top_grasps(block, under=True, tool_pose=unit_pose()) grasps = get_side_grasps(block, under=True, tool_pose=unit_pose()) for i, grasp_pose in enumerate(grasps): block_pose = multiply(tool_pose, grasp_pose) set_pose(block, block_pose) close_until_collision(robot, gripper_joints, bodies=[block], open_conf=get_open_positions(robot, arm), closed_conf=get_closed_positions(robot, arm)) handles = draw_pose(block_pose) wait_if_gui('Grasp {}'.format(i)) remove_handles(handles)
def solve_collision_free(door, obstacle, max_iterations=100, step_size=math.radians(5), min_distance=2e-2, draw=True): joints = get_movable_joints(door) door_link = child_link_from_joint(joints[-1]) # print(get_com_pose(door, door_link)) # print(get_link_inertial_pose(door, door_link)) # print(get_link_pose(door, door_link)) # draw_pose(get_com_pose(door, door_link)) handles = [] success = False start_time = time.time() for iteration in range(max_iterations): current_conf = np.array(get_joint_positions(door, joints)) collision_infos = get_closest_points(door, obstacle, link1=door_link, max_distance=min_distance) if not collision_infos: success = True break collision_infos = sorted(collision_infos, key=lambda info: info.contactDistance) collision_infos = collision_infos[:1] # TODO: average all these if draw: for collision_info in collision_infos: handles.extend(draw_collision_info(collision_info)) wait_if_gui() [collision_info] = collision_infos[:1] distance = collision_info.contactDistance print( 'Iteration: {} | Collisions: {} | Distance: {:.3f} | Time: {:.3f}'. format(iteration, len(collision_infos), distance, elapsed_time(start_time))) if distance >= min_distance: success = True break # TODO: convergence or decay in step size direction = step_size * get_unit_vector( collision_info.contactNormalOnB) # B->A (already normalized) contact_point = collision_info.positionOnA #com_pose = get_com_pose(door, door_link) # TODO: be careful here com_pose = get_link_pose(door, door_link) local_point = tform_point(invert(com_pose), contact_point) #local_point = unit_point() translate, rotate = compute_jacobian(door, door_link, point=local_point) delta_conf = np.array([ np.dot(translate[mj], direction) # + np.dot(rotate[mj], direction) for mj in movable_from_joints(door, joints) ]) new_conf = current_conf + delta_conf if violates_limits(door, joints, new_conf): break set_joint_positions(door, joints, new_conf) if draw: wait_if_gui() remove_handles(handles) print('Success: {} | Iteration: {} | Time: {:.3f}'.format( success, iteration, elapsed_time(start_time))) #quit() return success
def iterate(self, state): handles = self.draw() steps = int(math.ceil(self.duration / 0.02)) for _ in range(steps): yield remove_handles(handles)
def main(num_iterations=10): # The URDF loader seems robust to package:// and slightly wrong relative paths? connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") side = 0.05 block = create_box(w=side, l=side, h=side, color=RED) start_time = time.time() with LockRenderer(): with HideOutput(): # TODO: MOVO must be loaded last robot = load_model(MOVO_URDF, fixed_base=True) #set_all_color(robot, color=MOVO_COLOR) assign_link_colors(robot) base_joints = joints_from_names(robot, BASE_JOINTS) draw_base_limits((get_min_limits( robot, base_joints), get_max_limits(robot, base_joints)), z=1e-2) print('Load time: {:.3f}'.format(elapsed_time(start_time))) dump_body(robot) #print(get_colliding(robot)) #for arm in ARMS: # test_close_gripper(robot, arm) #test_grasps(robot, block) arm = RIGHT tool_link = link_from_name(robot, TOOL_LINK.format(arm)) #joint_names = HEAD_JOINTS #joints = joints_from_names(robot, joint_names) joints = base_joints + get_arm_joints(robot, arm) #joints = get_movable_joints(robot) print('Joints:', get_joint_names(robot, joints)) ik_info = MOVO_INFOS[arm] print_ik_warning(ik_info) ik_joints = get_ik_joints(robot, ik_info, tool_link) #fixed_joints = [] fixed_joints = ik_joints[:1] #fixed_joints = ik_joints wait_if_gui('Start?') sample_fn = get_sample_fn(robot, joints) handles = [] for i in range(num_iterations): conf = sample_fn() print('Iteration: {}/{} | Conf: {}'.format(i + 1, num_iterations, np.array(conf))) set_joint_positions(robot, joints, conf) tool_pose = get_link_pose(robot, tool_link) remove_handles(handles) handles = draw_pose(tool_pose) wait_if_gui() #conf = next(ikfast_inverse_kinematics(robot, MOVO_INFOS[arm], tool_link, tool_pose, # fixed_joints=fixed_joints, max_time=0.1), None) #if conf is not None: # set_joint_positions(robot, ik_joints, conf) #wait_if_gui() test_retraction(robot, ik_info, tool_link, fixed_joints=fixed_joints, max_time=0.05, max_candidates=100) disconnect()
def follow_curve_old(robot, joints, curve, goal_t=None): # TODO: unify with /Users/caelan/Programs/open-world-tamp/open_world/simulation/control.py if goal_t is None: goal_t = curve.x[-1] time_step = get_time_step() target_step = 10 * time_step #distance_fn = get_distance_fn(robot, joints, weights=None, norm=2) distance_fn = get_duration_fn(robot, joints, velocities=MAX_VELOCITIES, norm=INF) for i in irange(INF): # if (i % 10) != 0: # continue current_p = np.array(get_joint_positions(robot, joints)) current_v = np.array(get_joint_velocities(robot, joints)) goal_dist = distance_fn(current_p, curve(goal_t)) print('Positions: {} | Velocities: {} | Goal distance: {:.3f}'.format( current_p.round(N_DIGITS), current_v.round(N_DIGITS), goal_dist)) if goal_dist < 1e-2: return True # _, connection = mpc(current_p, current_v, curve, v_max=MAX_VELOCITIES, a_max=MAX_ACCELERATIONS, # dt_max=1e-1, max_time=1e-1) # assert connection is not None # target_t = 0.5*connection.x[-1] # target_p = connection(target_t) # target_v = connection(target_t, nu=1) # #print(target_p) closest_dist, closest_t = find_closest(current_p, curve, t_range=None, max_time=1e-2, max_iterations=INF, distance_fn=distance_fn, verbose=True) target_t = min(closest_t + target_step, curve.x[-1]) target_p = curve(target_t) #target_v = curve(target_t, nu=1) target_v = curve(closest_t, nu=1) #target_v = MAX_VELOCITIES #target_v = INF*np.zeros(len(joints)) handles = draw_waypoint(target_p) #times, confs = time_discretize_curve(curve, verbose=False, resolution=resolutions) # max_velocities=v_max, # set_joint_positions(robot, joints, target_p) max_velocity_control_joints(robot, joints, positions=target_p, velocities=target_v, max_velocities=MAX_VELOCITIES) #next_t = closest_t + time_step #next_p = current_p + current_v*time_step yield target_t actual_p = np.array(get_joint_positions(robot, joints)) actual_v = np.array(get_joint_velocities(robot, joints)) next_p = current_p + actual_v * time_step print('Predicted: {} | Actual: {}'.format(next_p.round(N_DIGITS), actual_p.round(N_DIGITS))) remove_handles(handles)