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)
test_create_path_constraints.py 文件源码
python
阅读 20
收藏 0
点赞 0
评论 0
评论列表
文章目录