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)
