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