用标量角张量创建变换矩阵

Question

原始问题

我想使用 keras 创建一个自定义 Lambda 函数，该函数执行关节臂的正向运动学。

这个函数有一组角度作为输入，应该输出一个包含末端执行器的位置和方向的向量。

我可以很容易地在 numpy 中创建这个函数；但是当我想把它移到 Keras 时，事情就变得困难了。

由于 lambda 函数的输入和输出都是张量，所以所有操作都应该使用张量和后端操作来完成。

问题是我必须从输入角度创建一个变换矩阵。

我可以使用K.cos 和K.sin（K 是后端张量流）来计算角度的余弦和正弦。但问题是如何创建一个 4X4 矩阵的张量，其中包含一些单元格，这些单元格只是数字（0 或1），而其他单元格是张量的一部分。 例如对于 Z 旋转：

T = tf.convert_to_tensor( [[c, -s, 0, dX],
                           [s,  c, 0, dY],
                           [0,  0, 1, dZ],
                           [0,  0, 0, 1]])

这里c 和s 是使用K.cos(input[3]) 和K.sin(input[3]) 计算的。 这不起作用。我明白了：

ValueError: Shapes must be equal rank, but are 1 and 0 将形状 1 与其他形状合并。对于 'lambda_1/packed/0' (op: 'Pack')，输入形状为：[5]、[5]、[]、[]。

有什么建议吗？

---

进一步的问题

@Aldream 提供的代码运行良好。 问题是当我将它嵌入到 Lambda 层时，编译模型时出现错误。

...
self.model.add(Lambda(self.FK_Keras))
self.model.compile(optimizer="adam", loss='mse', metrics=['mse'])

如您所见，我使用了一个包含模型和各种功能的类。 首先，我有一个计算变换矩阵的辅助函数：

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

然后 FK_keras 计算末端执行器变换：

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

这里 self.base_pos_K 和其他翻译向量是常量：

self.base_pos_K = K.constant(np.array([x,y,z]))

Tle 代码卡在编译函数中并返回此错误：

ValueError: Shapes must be equal rank, but are 1 and 0 将形状 1 与其他形状合并。对于 'lambda_1/stack_1' (op: 'Pack')，输入形状为：[5]、[5]、[]、[]。

我试图创建一个这样的快速测试代码：

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

此代码可以正常工作，没有错误。 可以将其集成到顺序模型的 Lambda 层中。

问题已解决

确实，问题与 Keras 处理数据的方式有关。它添加了一个批处理维度，在实现该功能时应考虑该维度。

我以不同的方式处理这个问题，包括重新实现 T_matrix_K 来处理这个额外的维度，但我认为@Aldream 提出的方式更优雅。

非常感谢@Aldream。他的回答很有帮助。

Answer 1

使用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 层期待/处理批处理数据。例如，Keras 会假设 Lambda(FK_Keras) 层的输入 (angs) 的形状为 (batch_size, 5)。因此，您的FK_Keras() 需要适应此类输入。

一种相当简单的方法是使用K.map_fn() 循环处理批处理中的每个角度列表，并对每个角度应用正确的T_matrix_K() 函数，只需对T_matrix_K() 进行少量编辑即可。

处理批次的其他小改动：

• 使用K.batch_dot() 代替K.dot()
• 相应地广播您的常量张量，例如self.base_pos_K
• 考虑到批量张量的附加第一维，例如将end_effector_T[1,0] 替换为end_effector_T[:, 1,0]

在下面找到一个缩短的工作代码（延伸到所有关节由您决定）：

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