def test_jacobian_prismatic(): chain = pk.build_serial_chain_from_urdf( open("prismatic_robot.urdf").read(), "link4") th = torch.zeros(3) tg = chain.forward_kinematics(th) m = tg.get_matrix() pos = m[0, :3, 3] assert torch.allclose(pos, torch.tensor([0, 0, 1.])) th = torch.tensor([0, 0.1, 0]) tg = chain.forward_kinematics(th) m = tg.get_matrix() pos = m[0, :3, 3] assert torch.allclose(pos, torch.tensor([0, -0.1, 1.])) th = torch.tensor([0.1, 0.1, 0]) tg = chain.forward_kinematics(th) m = tg.get_matrix() pos = m[0, :3, 3] assert torch.allclose(pos, torch.tensor([0, -0.1, 1.1])) th = torch.tensor([0.1, 0.1, 0.1]) tg = chain.forward_kinematics(th) m = tg.get_matrix() pos = m[0, :3, 3] assert torch.allclose(pos, torch.tensor([0.1, -0.1, 1.1])) J = chain.jacobian(th) assert torch.allclose( J, torch.tensor([[[0., 0., 1.], [0., -1., 0.], [1., 0., 0.], [0., 0., 0.], [0., 0., 0.], [0., 0., 0.]]]))
def test_jacobian_at_different_loc_than_ee(): chain = pk.build_serial_chain_from_urdf(open("kuka_iiwa.urdf").read(), "lbr_iiwa_link_7") th = torch.tensor([0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0]) loc = torch.tensor([0.1, 0, 0]) J = chain.jacobian(th, locations=loc) J_c1 = torch.tensor([[[-0., 0.11414214, -0., 0.18284271, 0., 0.1, 0.], [-0.66082756, -0., -0.38656497, -0., 0.12798633, -0., 0.1], [-0., 0.66082756, -0., -0.36384271, 0., 0.081, -0.], [-0., -0., -0.70710678, -0., -0.70710678, 0., -1.], [0., 1., 0., -1., 0., 1., 0.], [1., 0., 0.70710678, 0., -0.70710678, -0., 0.]]]) assert torch.allclose(J, J_c1) loc = torch.tensor([-0.1, 0.05, 0]) J = chain.jacobian(th, locations=loc) J_c2 = torch.tensor([[[-0.05, -0.08585786, -0.03535534, 0.38284271, 0.03535534, -0.1, -0.], [-0.66082756, -0., -0.52798633, -0., -0.01343503, 0., -0.1], [-0., 0.66082756, -0.03535534, -0.36384271, -0.03535534, 0.081, -0.05], [-0., -0., -0.70710678, -0., -0.70710678, 0., -1.], [0., 1., 0., -1., 0., 1., 0.], [1., 0., 0.70710678, 0., -0.70710678, -0., 0.]]]) assert torch.allclose(J, J_c2) # check that batching the location is fine th = th.repeat(2, 1) loc = torch.tensor([[0.1, 0, 0], [-0.1, 0.05, 0]]) J = chain.jacobian(th, locations=loc) assert torch.allclose(J, torch.cat((J_c1, J_c2)))
def test_parallel(): N = 100 chain = pk.build_serial_chain_from_urdf(open("kuka_iiwa.urdf").read(), "lbr_iiwa_link_7") th = torch.cat( (torch.tensor([[0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0]]), torch.rand(N, 7))) J = chain.jacobian(th) for i in range(N): J_i = chain.jacobian(th[i]) assert torch.allclose(J[i], J_i)
def test_dtype_device(): N = 1000 d = "cuda" if torch.cuda.is_available() else "cpu" dtype = torch.float64 chain = pk.build_serial_chain_from_urdf(open("kuka_iiwa.urdf").read(), "lbr_iiwa_link_7") chain = chain.to(dtype=dtype, device=d) th = torch.rand(N, 7, dtype=dtype, device=d) J = chain.jacobian(th) assert J.dtype is dtype
def test_gradient(): N = 10 d = "cuda" if torch.cuda.is_available() else "cpu" dtype = torch.float64 chain = pk.build_serial_chain_from_urdf(open("kuka_iiwa.urdf").read(), "lbr_iiwa_link_7") chain = chain.to(dtype=dtype, device=d) th = torch.rand(N, 7, dtype=dtype, device=d, requires_grad=True) J = chain.jacobian(th) assert th.grad is None J.norm().backward() assert th.grad is not None
def test_correctness(): chain = pk.build_serial_chain_from_urdf( open("kuka_iiwa.urdf").read(), "lbr_iiwa_link_7") th = torch.tensor( [0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0]) J = chain.jacobian(th) assert torch.allclose( J, torch.tensor([[[ -2.47727592e-12, 1.41421356e-02, -2.44155677e-12, 2.82842712e-01, 3.93156995e-13, 1.67535985e-14, 0.00000000e+00 ], [ -6.60827561e-01, -3.23289325e-12, -4.57275649e-01, -2.63812394e-12, 5.72756493e-02, 1.76135096e-13, 0.00000000e+00 ], [ 4.75722920e-17, 6.60827561e-01, -1.60872783e-12, -3.63842712e-01, 2.72085294e-13, 8.10000000e-02, 0.00000000e+00 ], [ -9.71829137e-17, -2.06797669e-13, -7.07106781e-01, -6.71813437e-12, -7.07106781e-01, 6.86429574e-12, -1.00000000e+00 ], [ -3.06245972e-16, 1.00000000e+00, 1.28796020e-12, -1.00000000e+00, 8.21292608e-12, 1.00000000e+00, 1.96776811e-12 ], [ 1.00000000e+00, 4.89681088e-12, 7.07106781e-01, 2.02812117e-12, -7.07106781e-01, -2.17450736e-12, 2.06834549e-13 ]]])) chain = pk.build_chain_from_sdf(open("simple_arm.sdf").read()) chain = pk.SerialChain(chain, "arm_wrist_roll_frame") th = torch.tensor([0.8, 0.2, -0.5, -0.3]) J = chain.jacobian(th) torch.allclose( J, torch.tensor([[[0., -1.51017878, -0.46280904, 0.], [0., 0.37144033, 0.29716627, 0.], [0., 0., 0., 0.], [0., 0., 0., 0.], [0., 0., 0., 0.], [0., 1., 1., 1.]]]))
def test_fkik(): data = '<robot name="test_robot">' \ '<link name="link1" />' \ '<link name="link2" />' \ '<link name="link3" />' \ '<joint name="joint1" type="revolute">' \ '<origin xyz="1.0 0.0 0.0"/>' \ '<parent link="link1"/>' \ '<child link="link2"/>' \ '</joint>' \ '<joint name="joint2" type="revolute">' \ '<origin xyz="1.0 0.0 0.0"/>' \ '<parent link="link2"/>' \ '<child link="link3"/>' \ '</joint>' \ '</robot>' chain = pk.build_serial_chain_from_urdf(data, 'link3') th1 = torch.tensor([0.42553542, 0.17529176]) tg = chain.forward_kinematics(th1) pos, rot = quat_pos_from_transform3d(tg) assert torch.allclose(pos, torch.tensor([[1.91081784, 0.41280851, 0.0000]])) assert quaternion_equality( rot, torch.tensor([[0.95521418, 0.0000, 0.0000, 0.2959153]])) print(tg) # TODO implement and test inverse kinematics # th2 = chain.inverse_kinematics(tg) # self.assertTrue(np.allclose(th1, th2, atol=1.0e-6)) # test batch kinematics N = 20 th_batch = torch.rand(N, 2) tg_batch = chain.forward_kinematics(th_batch) m = tg_batch.get_matrix() for i in range(N): tg = chain.forward_kinematics(th_batch[i]) assert torch.allclose(tg.get_matrix().view(4, 4), m[i]) # check that gradients are passed through th2 = torch.tensor([0.42553542, 0.17529176], requires_grad=True) tg = chain.forward_kinematics(th2) pos, rot = quat_pos_from_transform3d(tg) # note that since we are using existing operations we are not checking grad calculation correctness assert th2.grad is None pos.norm().backward() assert th2.grad is not None
def test_cuda(): if torch.cuda.is_available(): d = "cuda" dtype = torch.float64 chain = pk.build_chain_from_sdf(open("simple_arm.sdf").read()) chain = chain.to(dtype=dtype, device=d) ret = chain.forward_kinematics({ 'arm_elbow_pan_joint': math.pi / 2.0, 'arm_wrist_lift_joint': -0.5 }) tg = ret['arm_wrist_roll'] pos, rot = quat_pos_from_transform3d(tg) assert quaternion_equality( rot, torch.tensor([0.70710678, 0., 0., 0.70710678], dtype=dtype, device=d)) assert torch.allclose( pos, torch.tensor([1.05, 0.55, 0.5], dtype=dtype, device=d)) data = '<robot name="test_robot">' \ '<link name="link1" />' \ '<link name="link2" />' \ '<link name="link3" />' \ '<joint name="joint1" type="revolute">' \ '<origin xyz="1.0 0.0 0.0"/>' \ '<parent link="link1"/>' \ '<child link="link2"/>' \ '</joint>' \ '<joint name="joint2" type="revolute">' \ '<origin xyz="1.0 0.0 0.0"/>' \ '<parent link="link2"/>' \ '<child link="link3"/>' \ '</joint>' \ '</robot>' chain = pk.build_serial_chain_from_urdf(data, 'link3') chain = chain.to(dtype=dtype, device=d) N = 20 th_batch = torch.rand(N, 2).to(device=d, dtype=dtype) tg_batch = chain.forward_kinematics(th_batch) m = tg_batch.get_matrix() for i in range(N): tg = chain.forward_kinematics(th_batch[i]) assert torch.allclose(tg.get_matrix().view(4, 4), m[i])
def test_urdf(): chain = pk.build_serial_chain_from_urdf( open("kuka_iiwa.urdf").read(), "lbr_iiwa_link_7") print(chain) print(chain.get_joint_parameter_names()) th = [0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0] ret = chain.forward_kinematics(th, end_only=False) tg = ret['lbr_iiwa_link_7'] pos, rot = quat_pos_from_transform3d(tg) assert quaternion_equality( rot, torch.tensor([7.07106781e-01, 0, -7.07106781e-01, 0])) assert torch.allclose(pos, torch.tensor([-6.60827561e-01, 0, 3.74142136e-01])) N = 1000 d = "cuda" if torch.cuda.is_available() else "cpu" dtype = torch.float64 th_batch = torch.rand(N, len(chain.get_joint_parameter_names()), dtype=dtype, device=d) chain = chain.to(dtype=dtype, device=d) import time start = time.time() tg_batch = chain.forward_kinematics(th_batch) m = tg_batch.get_matrix() elapsed = time.time() - start print("elapsed {}s for N={} when parallel".format(elapsed, N)) start = time.time() elapsed = 0 for i in range(N): tg = chain.forward_kinematics(th_batch[i]) elapsed += time.time() - start start = time.time() assert torch.allclose(tg.get_matrix().view(4, 4), m[i]) print("elapsed {}s for N={} when serial".format(elapsed, N))
def test_correctness(): chain = pk.build_serial_chain_from_urdf(open("kuka_iiwa.urdf").read(), "lbr_iiwa_link_7") th = torch.tensor([0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0]) J = chain.jacobian(th) assert torch.allclose(J, torch.tensor([[[0, 1.41421356e-02, 0, 2.82842712e-01, 0, 0, 0], [-6.60827561e-01, 0, -4.57275649e-01, 0, 5.72756493e-02, 0, 0], [0, 6.60827561e-01, 0, -3.63842712e-01, 0, 8.10000000e-02, 0], [0, 0, -7.07106781e-01, 0, -7.07106781e-01, 0, -1], [0, 1, 0, -1, 0, 1, 0], [1, 0, 7.07106781e-01, 0, -7.07106781e-01, 0, 0]]])) chain = pk.build_chain_from_sdf(open("simple_arm.sdf").read()) chain = pk.SerialChain(chain, "arm_wrist_roll_frame") th = torch.tensor([0.8, 0.2, -0.5, -0.3]) J = chain.jacobian(th) torch.allclose(J, torch.tensor([[[0., -1.51017878, -0.46280904, 0.], [0., 0.37144033, 0.29716627, 0.], [0., 0., 0., 0.], [0., 0., 0., 0.], [0., 0., 0., 0.], [0., 1., 1., 1.]]]))