示例#1
0
def load_scene():
    table, b_base, b_tcp, c_base, c_tcp, cam_black, cam_white = SceneNode.n(7)
    table.adopt(b_base.adopt(b_tcp),
                c_base.adopt(c_tcp.adopt(cam_black, cam_white)))
    state = SceneState()
    state[table] = Transform()
    state[b_base] = Transform.load('calib/table_t_base_b')
    state[c_base] = Transform.load('calib/table_t_base_c')
    state[cam_black] = Transform.load('calib/tcp_t_cam_black')
    state[cam_white] = Transform.load('calib/tcp_t_cam_white')

    return Scene(table, b_base, b_tcp, c_base, c_tcp, cam_black,
                 cam_white), state
def load_grasp_config(grasp_order_desired: List[str] = None):
    kit_root = Path(__file__).parent.parent

    cad_files = json.load(open(kit_root / 'cad_files.json'))
    grasp_config = json.load(open(kit_root / 'grasping' / 'grasp_config.json'))
    min_clearance = grasp_config['grasp_other_clearance']

    if grasp_order_desired is None:
        grasp_order_desired = grasp_config['grasp_order_desired']

    obj_grasp_configs = []
    for name in grasp_order_desired:
        grasp_end_width = grasp_config['grasp_width'][name]
        grasp_start_width = grasp_end_width + from_dict(
            grasp_config['grasp_width_clearance'], name)
        sym_deg = grasp_config['grasp_sym_rot_deg'][name]
        sym_offset = from_dict(grasp_config['grasp_sym_offset'], name)

        mesh = trimesh.load_mesh(kit_root / 'stl' / f'{cad_files[name]}.stl')
        mesh.apply_scale(1e-3)
        tcp_t_obj = Transform.load(kit_root / 'grasping' / 'tcp_t_obj_grasp' /
                                   f'tcp_t_{name}_grasp')
        obj_grasp_configs.append((name, mesh, tcp_t_obj, grasp_start_width,
                                  grasp_end_width, sym_deg, sym_offset))

    return obj_grasp_configs, min_clearance
def main():
    import time
    from ..utils import load_tcp_t_cam, load_cam_intrinsics

    n = 40
    images = [cv2.imread(f'sweep/{i}.png') for i in tqdm(range(n))]
    base_t_tcps = [
        Transform.load(f'sweep/base_t_tcp_{i}') for i in tqdm(range(n))
    ]
    tcp_t_cam = load_tcp_t_cam('A')
    K = load_cam_intrinsics('A')[0]

    start = time.time()
    points_base = reconstruct_points(images=images,
                                     base_t_tcps=base_t_tcps,
                                     tcp_t_cam=tcp_t_cam,
                                     K=K,
                                     debug=False)
    print(f'{time.time() - start:.2f} s')

    print(points_base)
    np.save('points_base.npy', points_base)
# scripts/example_use_poses.py
from ur_control import Robot
from transform3d import Transform
import numpy as np

r = Robot.from_ip('192.168.1.130')
q_safe = (3.317805290222168, -1.3769071859172364, -1.9077906608581543,
          -1.4512933057597657, -4.752126518880026, -1.590332333241598)

base_t_taskboard = Transform.load('base_t_taskboard')
base_t_kitlayout = Transform.load('base_t_kitlayout')

board_t_tcp_desired = Transform(p=(0, 0, 0.24), rotvec=(np.pi, 0, 0))

base_t_tcp = base_t_taskboard @ board_t_tcp_desired
# uncomment the following line to move above the kitlayout frame
# base_t_tcp = base_t_kitlayout @ board_t_tcp_desired
r.ctrl.moveJ(q_safe)
r.ctrl.moveL(base_t_tcp)
示例#5
0
grasp_start_widths = {
    'big_round_peg': 25,
    'small_round_peg': 17,
    'big_gear': 40,
    'small_gear': 26,
    'bnc': 24,
    'belt': 30,
    'small_square_peg': 14,
    'big_square_peg': 22,
    'ethernet_cable_head': 22,
}

for obj_name in obj_names:
    r.ctrl.moveJ(q_safe)

    base_t_kit = Transform.load('base_t_kitlayout')
    kit_t_obj = Transform.load(f'kit_t_objects_practice/kit_t_{obj_name}')
    tcp_t_obj_grasp = Transform.load(f'tcp_t_obj_grasp/tcp_t_{obj_name}_grasp')
    base_t_tcp_grasp = base_t_kit @ kit_t_obj @ tcp_t_obj_grasp.inv

    r.ctrl.moveL(base_t_tcp_grasp @ Transform(p=(0, 0, -0.05)))
    gripper.move(grasp_start_widths[obj_name], 255, 255)
    r.ctrl.moveL(base_t_tcp_grasp)
    gripper.grasp(0, 0, 100, 0)
    if args.stop_at_grasp:
        quit()
    r.ctrl.moveL(r.base_t_tcp() @ Transform(p=(0, 0, -0.05)))
    if args.dont_put_back:
        quit()
    time.sleep(args.wait)
    r.ctrl.moveL(base_t_tcp_grasp)
示例#6
0
def load_table_t_base(name):
    return Transform.load(calib_folder / name / 'table_t_base')
示例#7
0
def load_tcp_t_cam(name):
    return Transform.load(calib_folder / name / 'tcp_t_cam')
示例#8
0
parser = argparse.ArgumentParser()
parser.add_argument('obj_name')
args = parser.parse_args()

obj_name = args.obj_name

gripper = get_gripper()
gripper.open()

r = Robot.from_ip('192.168.1.130')
q_safe = (3.317805290222168, -1.3769071859172364, -1.9077906608581543,
          -1.4512933057597657, -4.752126518880026, -1.590332333241598)
r.ctrl.moveJ(q_safe)

base_t_kit = Transform.load('base_t_kitlayout')

kit_t_obj = Transform.load(f'kit_t_objects_practice/kit_t_{obj_name}')

base_t_tcp = base_t_kit @ Transform(p=(*kit_t_obj.p[:2], 0.25),
                                    rotvec=(np.pi, 0, 0))
r.ctrl.moveL(base_t_tcp)

r.ctrl.teachMode()
input('enter when grasp pose is good')
r.ctrl.endTeachMode()

base_t_tcp = r.base_t_tcp()
tcp_t_obj = base_t_tcp.inv @ base_t_kit @ kit_t_obj

print('tcp_t_obj:')
示例#9
0
def from_dict(d, k):
    return d[k] if k in d else d['default']


parser = argparse.ArgumentParser()
parser.add_argument('--base-t-kitlayout', required=True)
parser.add_argument('--obj-names', nargs='+', default=None)
parser.add_argument('--layout', default=layouts.practice_fp)
parser.add_argument('--get-grasp-position', action='store_true')
parser.add_argument('--examine-grasp', action='store_true')
parser.add_argument('--stop-at-grasp', action='store_true')
parser.add_argument('--debug', action='store_true')
args = parser.parse_args()

base_t_kit = Transform.load(args.base_t_kitlayout)
kit_t_objects = extract_kit_t_objects(args.layout, debug=args.debug)
obj_grasp_configs, min_clearance = load_grasp_config(args.obj_names)
grasps, min_clearance = determine_grasps_decreasing_clearance(
    obj_grasp_configs, kit_t_objects, min_clearance, debug=args.debug)

gripper = get_gripper()
gripper.open()

r = Robot.from_ip('192.168.1.130')
r.ctrl.moveL(r.base_t_tcp() @ Transform(p=(0, 0, -0.05)))
q_safe = (2.8662071228027344, -1.7563158474364222, -1.9528794288635254,
          -1.0198443692973633, -4.752078358327047, -2.1280840078936976)
q_drop = (3.0432159900665283, -2.0951763592162074, -1.622988224029541,
          -0.9942649167827149, -4.733094040547506, -1.5899961630450647)
示例#10
0
import numpy as np
from scipy.spatial.kdtree import KDTree
from scipy.spatial.ckdtree import cKDTree
import trimesh
from transform3d import Transform

poses_gt = [
    ('monkey', Transform(rpy=(15, 30, 45), degrees=True)),
    ('torus', Transform(p=(-0.05, 0.07, 0),
                        rpy=(-140, -100, -80),
                        degrees=True)),
    ('offset_cylinder',
     Transform(p=(-0.3, -0.05, 0.05), rpy=(-128, -15, 35), degrees=True)),
]

for name, pose_gt in poses_gt:
    pose_est = Transform.load(f'scene.{name}.pose')

    mesh = trimesh.load_mesh(f'{name}.stl')
    hv = mesh.convex_hull.vertices
    diameter = np.linalg.norm(hv[None] - hv[:, None], axis=-1).max()

    v_gt = pose_gt @ mesh.vertices
    v_est = pose_est @ mesh.vertices

    tree_gt = cKDTree(v_gt)  # type: KDTree
    dists, _ = tree_gt.query(v_est)
    adds_score = np.mean(dists) / diameter
    print(f'{name}: {adds_score:.3f}')