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]
