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])
