human_model.py 文件源码

def nearest_neighbour(self, fk_dict, scale_orient=40):
        def query_database(link, fk):
            x_vector = deepcopy(fk[0])
            if self.with_orient:
                x_vector += (np.array(fk[1]) / scale_orient).tolist()
            res = self.trees[link]['tree'].query(x_vector)
            joints = self.trees[link]['data']['data'][res[1]][:-7]
            joint_names = self.trees[link]['data']['names'][:-7]
            return joints.tolist(), joint_names.tolist()

        state = JointState()
        for key, value in fk_dict.iteritems():
            joints, joint_names = query_database(key, value)
            state.position += joints
            state.name += joint_names
        return state