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

Question

原始问题

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

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

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

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

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

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

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

这里c和{}是使用K.cos(input[3])和{来计算的。 这不起作用。我得到：

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], [], [].

有什么建议吗？在

---

更多问题

@Aldream提供的代码运行良好。 问题是当我把这个嵌入到Lambda层中时，我在编译模型时得到了一个错误。在

^{pr2}$

如您所见，我使用了一个保存模型和各种函数的类。 首先，我有一个helper函数来计算转换矩阵：

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éu 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 From merging shape 1 with other shapes. for 'lambda_1/stack_1' (op: 'Pack') with input shapes: [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处理数据的方式有关。它添加了在实现函数时应该考虑的批处理维度。在

我想用另一种方法来处理这个矩阵。在

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

Answer 1

使用^{}：

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()需要进行调整以处理此类输入。在

一种相当简单的方法，只需要对T_matrix_K()进行小的编辑，就是使用K.map_fn()遍历批处理中的每个角度列表，并对每个角度应用适当的T_matrix_K()函数。在

处理批次的其他小变化：

• 使用K.batch_dot()代替K.dot()
• 根据你的常量张量广播，例如self.base_pos_K
• 考虑批处理张量的附加第一维，例如用end_effector_T[:, 1,0]替换end_effector_T[1,0]

在下面找到一个简短的工作代码（扩展到所有关节由您自己决定）：

^{pr2}$