Lab1-IK的part1_inverse_kinematics中梯度下降法更新参数时的顺序

[daidaiershidi]

Lab1-IK的part1_inverse_kinematics中，我的理解是：因为同时得到了joint_orientations_t和local_rotation_t，他俩都可以用于更新整个joint_orientations参数。目前代码里是先用local_rotation_t更新，然后在用joint_orientations_t去更新。

但joint_orientations_t中，joint_ik_path上的节点之间好像不够连贯，在我的代码里会得到类似这种结果：

我理解会造成这样结果的原因是joint_ik_path上的节点的joint_orientations是直接用joint_orientations_t赋值的，但joint_orientations_t没有身体结构的约束。不知道这样想对不对。

然后我就调换了一下更新的顺序：

    # for j in range(len(joint_ik_path)):
    #     a = chain_current = joint_ik_path[j]
    #     b = chain_parent = joint_ik_path[j - 1]
    #     if j == 0:
    #          local_rotation[a] = R.from_matrix(local_rotation_t[a].detach().numpy())
    #          joint_positions[a] = joint_positions[a]
    #     elif b == joint_parent[a]:  # 当前结点是前一结点的子节点，正向
    #          joint_orientations[a] = (R.from_quat(joint_orientations[b]) * R.from_matrix(local_rotation_t[a].detach().numpy())).as_quat()
    #          joint_positions[a] = joint_positions[b] + joint_offset[a] * np.asmatrix(R.from_quat(joint_orientations[b]).as_matrix()).transpose()
    #     else:  # a = joint_parent[b] 当前结点是前一节点的父结点，逆向
    #          joint_orientations[a] = (R.from_quat(joint_orientations[b]) * R.from_matrix(local_rotation_t[b].detach().numpy()).inv()).as_quat()
    #          joint_positions[a] = joint_positions[b] + (-joint_offset[b]) * np.asmatrix(R.from_quat(joint_orientations[a]).as_matrix()).transpose()

    # 我们获得了链条上每个关节的Orientation和Position，然后我们只需要更新非链上结点的位置
    ik_path_set = set(joint_ik_path)
    for i in range(len(joint_positions)):
        if i in ik_path_set:
            joint_orientations[i] = R.from_matrix(joint_orientations_t[i].detach().numpy()).as_quat()
        else:
            joint_orientations[i] = (R.from_quat(joint_orientations[joint_parent[i]]) * local_rotation[i]).as_quat()
            joint_positions[i] = joint_positions[joint_parent[i]] + joint_offset[i] * np.asmatrix(
                R.from_quat(joint_orientations[joint_parent[i]]).as_matrix()).transpose()

    for j in range(len(joint_ik_path)):
        a = chain_current = joint_ik_path[j]
        b = chain_parent = joint_ik_path[j - 1]
        if j == 0:
             local_rotation[a] = R.from_matrix(local_rotation_t[a].detach().numpy())
             joint_positions[a] = joint_positions[a]
        elif b == joint_parent[a]:  # 当前结点是前一结点的子节点，正向
             joint_orientations[a] = (R.from_quat(joint_orientations[b]) * R.from_matrix(local_rotation_t[a].detach().numpy())).as_quat()
             joint_positions[a] = joint_positions[b] + joint_offset[a] * np.asmatrix(R.from_quat(joint_orientations[b]).as_matrix()).transpose()
        else:  # a = joint_parent[b] 当前结点是前一节点的父结点，逆向
             joint_orientations[a] = (R.from_quat(joint_orientations[b]) * R.from_matrix(local_rotation_t[b].detach().numpy()).inv()).as_quat()
             joint_positions[a] = joint_positions[b] + (-joint_offset[b]) * np.asmatrix(R.from_quat(joint_orientations[a]).as_matrix()).transpose()

在我的代码里结果就正常了：

[DIOYF]

之前有个关闭的issue，有提到我FK计算有问题，代码有一段时间没看了，最近比较繁忙，抱歉，后续有时间我会再看下代码里面问题，我感觉可能我中间的计算有问题