def infer(self): Flow().defaults() model = Model().make_fcn() model.load_weights('model_weights.h5') chain = Chain().make() angle_scale = Scale([-1.0, 1.0], [-0.5, 0.5]) position_scale = Scale([-3.0, 3.0], [-0.5, 0.5]) # Reference angles theta1_ref = 0.3 theta2_ref = 0.4 theta3_ref = 0.35 ref = np.array([theta1_ref, theta2_ref, theta3_ref]) print('\nReference angles:') print(ref) # Forward kinematic positions px, py, pz = chain.forward({ 'theta1': theta1_ref, 'theta2': theta2_ref, 'theta3': theta3_ref }) print('\nComputed positions:') print(np.array([px, py, pz])) # Normalize the position px = position_scale.forward_scale(px) py = position_scale.forward_scale(py) pz = position_scale.forward_scale(pz) p = np.array([[px, py, pz]]) # Inference returns normalized angle r = model.predict(p) # De-normalized inference angles theta1_ik = angle_scale.reverse_scale(r[0][0]) theta2_ik = angle_scale.reverse_scale(r[0][1]) theta3_ik = angle_scale.reverse_scale(r[0][2]) inf = np.array([theta1_ik, theta2_ik, theta3_ik]) print('\nInferred IK angles:') print(inf) # Relative error err = np.abs(ref - inf) / ref print('\nAngle error (relative):') print(err)
class Generator(object): def __init__(self): self.chain = Chain().make() self.angle_scale = Scale([-1.0, 1.0], [-0.5, 0.5]) self.position_scale = Scale([-3.0, 3.0], [-0.5, 0.5]) def generate_angles(self): theta1 = np.random.uniform(-1.0, 1.0) theta2 = np.random.uniform(-1.0, 1.0) theta3 = np.random.uniform(-1.0, 1.0) return [theta1, theta2, theta3] def scale_angles(self, a): theta1 = self.angle_scale.forward_scale(a[0]) theta2 = self.angle_scale.forward_scale(a[1]) theta3 = self.angle_scale.forward_scale(a[2]) return [theta1, theta2, theta3] def generate_positions(self, a): p = self.chain.forward({ 'theta1': a[0], 'theta2': a[1], 'theta3': a[2] }) return ([p[0], p[1], p[2]]) def scale_positions(self, p): p0 = self.position_scale.forward_scale(p[0]) p1 = self.position_scale.forward_scale(p[1]) p2 = self.position_scale.forward_scale(p[2]) return [p0, p1, p2] def make(self, batch_size): while 1: features = [] labels = [] for i in range(batch_size): angles = self.generate_angles() positions = self.generate_positions(angles) features.append(self.scale_positions(positions)) labels.append(self.scale_angles(angles)) yield np.array([features, labels])