test_create_path_constraints.py 文件源码

python
阅读 20 收藏 0 点赞 0 评论 0

项目:toppra 作者: hungpham2511 项目源码 文件源码
def test_null_space(self, rave_re_torque_data):
        """pc.D[i] must equals N.T, where N is orthogonal
        to the loop closure Jacobian.
        """
        data, pc = rave_re_torque_data
        pi, ss, robot, J_lc = data

        # Assert nullspace matrix at s index = 0
        q = pi.eval(ss)
        for i in range(pc.N + 1):
            Nmat = pc.D[i].T
            qi = q[i]
            nr, nc = Nmat.shape
            for i in range(nc):
                _ = np.dot(J_lc(qi), Nmat[:, i])
                npt.assert_allclose(np.linalg.norm(_), 0, atol=TINY)
评论列表
文章目录


问题


面经


文章

微信
公众号

扫码关注公众号