def from_file(cls, fp): fp_dir = os.path.dirname(fp) scene = cls() data = json.load(open(fp)) for d in data['cameras']: scene.cameras[d['name']] = Camera(np.array(d['matrix']), d['width'], d['height']) for d in data['objects']: scene.objects.append( Object( d['name'], os.path.join(fp_dir, d['file_path']), Transform.from_xyz_rotvec(d['pose']) if 'pose' in d else None, d['scale'] if 'scale' in d else 1, )) images = [] for d in data['images']: images.append( Image(os.path.join(fp_dir, d['file_path']), scene.cameras[d['camera']], Transform.from_xyz_rotvec(d['camera_pose']))) order = utils.farthest_point_sampling( np.array([img.camera_pose.p for img in images]), len(images)) for i in order: scene.images.append(images[i]) return scene
def prune(demonstration, min_dist=0.01, min_angle=np.deg2rad(10), min_q_dist=0.01, use_q_space=True): idxs = [0] for i, (q, pose) in enumerate(demonstration): if use_q_space: q_last = demonstration[idxs[-1]][0] if np.linalg.norm(q_last - q) > min_q_dist: idxs.append(i) else: T_last = Transform.from_xyz_rotvec(demonstration[idxs[-1]][1]) T = Transform.from_xyz_rotvec(pose) diff = T @ T_last.inv() dist, angle = np.linalg.norm(diff.t), np.linalg.norm( diff.r.as_rotvec()) if dist > min_dist or angle > min_angle: idxs.append(i) return np.array(idxs)
def spiral_search(self, tol: float, max_radius: float, push_force=5., speed=0.01, ft_stop=(15., 15., np.inf, 2., 2., 2.), stop_condition=lambda: False, lookahead_time=0.1, gain=600): # TODO: acc limit # TODO: insertion direction try: base_t_tcp_init = self.base_t_tcp() self.ctrl.forceModeStart(base_t_tcp_init, [0, 0, 1, 0, 0, 0], [0, 0, push_force, 0, 0, 0], 2, [0.05] * 6) a, b = spiral_utils.get_spiral_params(tol) theta = 0 while True: loop_start = time.time() theta = spiral_utils.next_theta_constant_speed(theta, a, b, speed=speed, dt=self.dt) r = spiral_utils.radius_from_theta(a, b, theta) if r > max_radius: raise DeviatingMotionError() x, y = np.cos(theta) * r - a, np.sin(theta) * r base_t_tcp = Transform.from_xyz_rotvec( self.recv.getActualTCPPose()) ft_tcp = base_t_tcp.inv.rotate(self.recv.getActualTCPForce()) if any(np.abs(ft_tcp) > ft_stop): return TerminateReason.FORCE_TORQUE_LIMIT elif stop_condition(): return TerminateReason.STOP_CONDITION tcp_start_t_tcp_next = Transform(p=(x, y, 0)) base_t_tcp_next = base_t_tcp_init @ tcp_start_t_tcp_next self.ctrl.servoL(base_t_tcp_next, 0.5, 0.25, self.dt, lookahead_time, gain) loop_duration = time.time() - loop_start time.sleep(max(0., self.dt - loop_duration)) finally: self.ctrl.servoStop() self.ctrl.forceModeStop()
def main(): # record_data('data.npy', zero_payload=True) # quit() data = np.load('data.npy') base_t_tcps = [Transform.from_xyz_rotvec(d[:6]) for d in data] ft_tcps = [d[6:] for d in data] tool_mass, tcp_p_cog = calibrate_payload(base_t_tcps, ft_tcps) print(tool_mass, tcp_p_cog) r = Robot.from_ip('192.168.1.130') r.ctrl.setPayload(tool_mass, tcp_p_cog) data_after = np.load('data.npy') print('force max', np.abs(data_after[:, 6:9]).max()) print('force mean', np.abs(data_after[:, 6:9]).mean()) print('force rms', np.sqrt((data_after[:, 6:9]**2).mean())) print() print('torque max', np.abs(data_after[:, 9:]).max()) print('torque mean', np.abs(data_after[:, 9:]).mean())
def base_t_tcp(self): # TODO: possibly cache by default with a ttl ~≃ self.dt return Transform.from_xyz_rotvec(self.recv.getActualTCPPose())
from data_handling.wrs_persistence import WRSPersistence from i40.loaders.cell_loader import CellLoader from transform3d import Transform persistence = WRSPersistence.driver() cell = CellLoader.load(persistence) table_t_base_b = Transform.from_xyz_rotvec( cell.find_serial_device("robot_b").world_t_base()) table_t_base_b.save('table_t_base_b')
from calibrate_cam import CameraIntrinsics import park_martin parser = argparse.ArgumentParser() parser.add_argument('--folder', required=True) args = parser.parse_args() folder = Path(args.folder) calib = CameraIntrinsics(**json.load(open(folder / 'camera_intrinsics.json'))) rvecs, tvecs = np.array(calib.rvecs)[..., 0], np.array(calib.tvecs)[..., 0] base_t_tcps = [ Transform.from_xyz_rotvec(d) for d in np.load(folder / 'image_tcp_poses.npy')[calib.img_ids] ] cam_t_boards = [ Transform(p=tvec, rotvec=rvec) for rvec, tvec in zip(rvecs, tvecs) ] n = len(cam_t_boards) assert n == len(base_t_tcps), '{}, {}'.format(n, len(base_t_tcps)) A, B = [], [] shuffle_idx = np.arange(n) for j in range(10): np.random.RandomState(seed=j).shuffle(shuffle_idx) for i in range(1, n): a, b = shuffle_idx[i], shuffle_idx[i - 1]