def test_conversion_panda(original_pose): pose_info = [ ( [0, 1, 0], 0, ), ( [0, 1, 0], 180, ), ( [0, 1, 0], 180, ), ( [0, 1], 90, ), ] for position, heading in pose_info: heading = Heading.from_panda3d(heading) p_from_center = Pose.from_center(base_position=position, heading=heading) assert len(p_from_center.position) == 3 assert len(p_from_center.orientation) == 4 a_pos, a_ori = p_from_center.position, p_from_center.heading if len(position) < 3: position = [*position, 0] assert np.isclose(position, a_pos, atol=2e-07).all() assert math.isclose(heading, a_ori, abs_tol=2e-07)
def social_spin_on_centre_ccw(step, position): return Pose.from_center(np.array(position), Heading.from_panda3d(step))
def heading(): return Heading.from_panda3d(-250)