(joint_id, kinematic_tree)
| 85 | |
| 86 | |
| 87 | def find_joint_kin_chain(joint_id, kinematic_tree): |
| 88 | kin_chain = [] |
| 89 | curr_idx = joint_id |
| 90 | while curr_idx != -1: |
| 91 | kin_chain.append(curr_idx) |
| 92 | curr_idx = kinematic_tree[curr_idx] |
| 93 | return kin_chain |
| 94 | |
| 95 | |
| 96 | def to_tensor(array: Union[Array, Tensor], dtype=torch.float32) -> Tensor: |