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)
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)
def load_table_t_base(name):
return Transform.load(calib_folder / name / 'table_t_base')
def load_tcp_t_cam(name):
return Transform.load(calib_folder / name / 'tcp_t_cam')
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:')
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)
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}')