def social_spin_on_axle_cw(step, base_position, offset_from_centre):
return Pose.from_explicit_offset(
offset_from_centre,
np.array(base_position),
Heading.from_sumo(step),
local_heading=Heading(0),
)
def test_conversion_bullet(original_pose):
pose_info = [
(
[2, 0, 0],
0,
[-2, 0, 0],
),
([-1, 0, 0], math.pi, [-1, 0, 0]),
(
[0, -1.5, 0],
math.pi * 0.5,
[-1.5, 0, 0],
),
]
for offset_to_center, heading, position_offset in pose_info:
heading = Heading.from_bullet(heading)
offset_to_center = np.array(offset_to_center)
base_position = np.sum([position_offset, original_pose.position],
axis=0)
p_from_explicit_offset = Pose.from_explicit_offset(
offset_from_centre=offset_to_center,
base_position=base_position,
heading=heading,
local_heading=Heading(0),
)
assert np.isclose(p_from_explicit_offset.position,
original_pose.position,
atol=2e-07).all()
assert math.isclose(p_from_explicit_offset.position[0],
original_pose.position[0],
abs_tol=2e-07)
def pose(self) -> Pose:
pos, orn = self._client.getBasePositionAndOrientation(self._bullet_id)
heading = Heading(yaw_from_quaternion(orn))
# NOTE: we're inefficiently creating a new Pose object on every call here,
# but it's too risky to change this because our clients now rely on this behavior.
return Pose.from_explicit_offset(
[0, 0, 0],
np.array(pos),
heading,
local_heading=Heading(0),
)
def pose(self) -> Pose:
pos, orn = self._client.getBasePositionAndOrientation(self._bullet_id)
heading = Heading(yaw_from_quaternion(orn))
pose = Pose.from_explicit_offset(
[0, 0, 0],
np.array(pos),
heading,
local_heading=Heading(0),
)
return pose
