def test_inverse_frame(self, x, y, z, q):
r1 = np.array(spw.inverse_frame(spw.frame_quaternion(x, y, z, q[0], q[1], q[2], q[3]))).astype(float)
r2 = PyKDL.Frame()
r2.M = PyKDL.Rotation.Quaternion(q[0], q[1], q[2], q[3])
r2.p[0] = x
r2.p[1] = y
r2.p[2] = z
r2 = r2.Inverse()
r2 = pykdl_frame_to_numpy(r2)
self.assertTrue(np.isclose(r1, r2).all(), msg='\n{} != \n{}'.format(r1, r2))
def test_donbot_fk1(self, js):
r = Robot.from_urdf_file(body_urdf)
kdl = KDL(body_urdf)
root = u'base_footprint'
tips = [u'left_gripper_tool_frame', u'right_gripper_tool_frame']
for tip in tips:
kdl_r = kdl.get_robot(root, tip)
kdl_fk = kdl_r.fk_np(js)
symengine_fk = r.get_fk_expression(root, tip).subs(js)
np.testing.assert_array_almost_equal(kdl_fk,
symengine_fk,
decimal=3)
np.testing.assert_array_almost_equal(
kdl_r.fk_np_inv(js), sw.inverse_frame(symengine_fk), decimal=3)
def cb_states(states_msg):
global map_trans, map_quat
try:
idx = states_msg.name.index(robot_name)
pose = states_msg.pose[idx]
map_base_link = inverse_frame(
frame3_quaternion(pose.position.x, pose.position.y, 0,
pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w))
map_trans = map_base_link[:3, 3]
map_quat = real_quat_from_matrix(map_base_link)
except (ValueError, tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
pass