I want to create a custom Lambda function using keras that does the forward kinematics of an articulated arm.
This function has a set of angles as input and should output a vector containing the position and orientation of the end effector.
I could create this function in numpy easily; but when I wanted to move it to Keras, things got hard.
Since the input and the output of the lambda function are tensors, all operations should be done using tensors and the backend operations.
The problem is that I have to create a transformation matrix out of the input angles.
I could use K.cos and K.sin (K is the backend tensorflow) to compute the cosines and sines of the angles. But the problem is how to create a tensor that is a 4X4 matrix that contains some cells which are just numbers (0 or 1) and the others are parts of a tensor.
For example for a Z rotation :
T = tf.convert_to_tensor( [[c, -s, 0, dX],
[s, c, 0, dY],
[0, 0, 1, dZ],
[0, 0, 0, 1]])
Here c and s are computed using K.cos(input[3]) and K.sin(input[3]).
This does not work. I get :
ValueError: Shapes must be equal rank, but are 1 and 0 From merging shape 1 with other shapes. for 'lambda_1/packed/0' (op: 'Pack') with input shapes: [5], [5], [], [].
Any suggestions?
The code provided by @Aldream did work fine. The problem is when I embed this into a Lambda layer, I get an error when I compile the model.
...
self.model.add(Lambda(self.FK_Keras))
self.model.compile(optimizer="adam", loss='mse', metrics=['mse'])
As you can see, I use a class that holds the model and the various functions. First I have a helper function That computes the transformation matrix:
def trig_K( angle):
r = angle*np.pi/180.0
return K.cos(r), K.sin(r)
def T_matrix_K(rotation, axis="z", translation=K.constant([0,0,0])):
c, s = trig_K(rotation)
dX = translation[0]
dY = translation[1]
dZ = translation[2]
if(axis=="z"):
T = K.stack( [[c, -s, 0., dX],
[s, c, 0., dY],
[0., 0., 1., dZ],
[0., 0., 0., 1.]], axis=0)
if(axis=="y"):
T = K.stack( [ [c, 0.,-s, dX],
[0., 1., 0., dY],
[s, 0., c, dZ],
[0., 0., 0., .1]], axis=0)
if(axis=="x"):
T = K.stack( [ [1., 0., 0., dX],
[0., c, -s, dY],
[0., s, c, dZ],
[0., 0., 0., 1.]], axis=0)
return T
Then FK_keras computes the end effector transformation:
def FK_Keras(self, angs):
# Compute local transformations
base_T=T_matrix_K(angs[0],"z",self.base_pos_K)
shoulder_T=T_matrix_K(angs[1],"y",self.shoulder_pos_K)
elbow_T=T_matrix_K(angs[2],"y",self.elbow_pos_K)
wrist_1_T=T_matrix_K(angs[3],"y",self.wrist_1_pos_K)
wrist_2_T=T_matrix_K(angs[4],"x",self.wrist_2_pos_K)
# Compute end effector transformation
end_effector_T=K.dot(base_T,K.dot(shoulder_T,K.dot(elbow_T,K.dot(wrist_1_T,wrist_2_T))))
# Compute Yaw, Pitch, Roll of end effector
y=K.tf.atan2(end_effector_T[1,0],end_effector_T[1,1])
p=K.tf.atan2(-end_effector_T[2,0],K.tf.sqrt(end_effector_T[2,1]*end_effector_T[2,1]+end_effector_T[2,2]*end_effector_T[2,2]))
r=K.tf.atan2(end_effector_T[2,1],end_effector_T[2,2])
# Construct the output tensor [x,y,z,y,p,r]
output = K.stack([end_effector_T[0,3],end_effector_T[1,3],end_effector_T[2,3], y, p, r], axis=0)
return output
Here self.base_pos_K and the other translations vectors are constants :
self.base_pos_K = K.constant(np.array([x,y,z]))
Tle code stucks in the compile function and return this error :
ValueError: Shapes must be equal rank, but are 1 and 0 From merging shape 1 with other shapes. for 'lambda_1/stack_1' (op: 'Pack') with input shapes: [5], [5], [], [].
I tried to create a fast test code like this :
arm = Bot("")
# Articulation angles
input_data =np.array([90., 180., 45., 25., 25.])
sess = K.get_session()
inp = K.placeholder(shape=(5), name="inp")#)
res = sess.run(arm.FK_Keras(inp),{inp: input_data})
This code do works with no errors. There is something about integrating this into a Lambda layer of a sequential model.
Indeed, the problem was related to the way Keras deals with data. It adds a batch dimension which should be taken into consideration while implmenting the function.
I dealt with this in a different way which involved reimplementing the T_matrix_K to deal with this extra dimension, but I think the way proposed by @Aldream is more elegent.
Many thanks to @Aldream. His answers were quite helpful.
Using K.stack():
import keras
import keras.backend as K
input = K.constant([3.14, 0., 0, 3.14])
dX, dY, dZ = K.constant(1.), K.constant(2.), K.constant(3.)
c, s = K.cos(input[3]), K.sin(input[3])
T = K.stack([[ c, -s, 0., dX],
[ s, c, 0., dY],
[0., 0., 1., dZ],
[0., 0., 0., 1.]], axis=0
)
sess = K.get_session()
res = sess.run(T)
print(res)
# [[ -9.99998748e-01 -1.59254798e-03 0.00000000e+00 1.00000000e+00]
# [ 1.59254798e-03 -9.99998748e-01 0.00000000e+00 2.00000000e+00]
# [ 0.00000000e+00 0.00000000e+00 1.00000000e+00 3.00000000e+00]
# [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
Lambda:Keras layers are expecting/dealing with batched data. Keras would for instance assume that the input (angs) of your Lambda(FK_Keras) layer is of shape (batch_size, 5). Your FK_Keras() thus need to be adapted to deal with such inputs.
A rather straightforward way to do so, requiring only minor edits to your T_matrix_K() is to use K.map_fn() to loop over every list of angles in the batch and apply the proper T_matrix_K() function to each.
Other minor changes to deal with batches:
K.batch_dot() instead of K.dot()self.base_pos_Kend_effector_T[1,0] by end_effector_T[:, 1,0]Find below a shortened working code (extending to all joints is left to you):
import keras
import keras.backend as K
from keras.layers import Lambda, Dense
from keras.models import Model, Sequential
import numpy as np
def trig_K( angle):
r = angle*np.pi/180.0
return K.cos(r), K.sin(r)
def T_matrix_K_z(x):
rotation, translation = x[0], x[1]
c, s = trig_K(rotation)
T = K.stack( [[c, -s, 0., translation[0]],
[s, c, 0., translation[1]],
[0., 0., 1., translation[2]],
[0., 0., 0., 1.]], axis=0)
# We have 2 inputs, so have to return 2 outputs for `K.map_fn()`:
return T, 0.
def T_matrix_K_y(x):
rotation, translation = x[0], x[1]
c, s = trig_K(rotation)
T = K.stack( [ [c, 0.,-s, translation[0]],
[0., 1., 0., translation[1]],
[s, 0., c, translation[2]],
[0., 0., 0., .1]], axis=0)
# We have 2 inputs, so have to return 2 outputs for `K.map_fn()`:
return T, 0.
def FK_Keras(angs):
base_pos_K = K.constant(np.array([1, 2, 3])) # replace with your self.base_pos_K
shoulder_pos_K = K.constant(np.array([1, 2, 3])) # replace with your self.shoulder_pos_K
# Manually broadcast your constants to batches:
batch_size = K.shape(angs)[0]
base_pos_K = K.tile(K.expand_dims(base_pos_K, 0), (batch_size, 1))
shoulder_pos_K = K.tile(K.expand_dims(shoulder_pos_K, 0), (batch_size, 1))
# Compute local transformations, for each list of angles in the batch:
base_T, _ = K.map_fn(T_matrix_K_z, (angs[:, 0], base_pos_K))
shoulder_T, _ = K.map_fn(T_matrix_K_y, (angs[:, 1], shoulder_pos_K))
# ... (repeat with your other joints)
# Compute end effector transformation, over batch:
end_effector_T = K.batch_dot(base_T,shoulder_T) # add your other joints
# Compute Yaw, Pitch, Roll of end effector
y=K.tf.atan2(end_effector_T[:, 1,0],end_effector_T[:, 1,1])
p=K.tf.atan2(-end_effector_T[:, 2,0],K.tf.sqrt(end_effector_T[:, 2,1]*end_effector_T[:, 2,1]+end_effector_T[:, 2,2]*end_effector_T[:, 2,2]))
r=K.tf.atan2(end_effector_T[:, 2,1],end_effector_T[:, 2,2])
# Construct the output tensor [x,y,z,y,p,r]
output = K.stack([end_effector_T[:, 0,3],end_effector_T[:, 1,3],end_effector_T[:, 2,3], y, p, r], axis=1)
return output
# Demonstration:
input_data =np.array([[90., 180., 45., 25., 25.],[90., 180., 45., 25., 25.]])
sess = K.get_session()
inp = K.placeholder(shape=(None, 5), name="inp")#)
res = sess.run(FK_Keras(inp),{inp: input_data})
model = Sequential()
model.add(Dense(5, input_dim=5))
model.add(Lambda(FK_Keras))
model.compile(optimizer="adam", loss='mse', metrics=['mse'])