def test_conversion(self):
# Added seed to make tests with np.random deterministic
np.random.seed(0)
for _ in range(50):
# Creating random vector
vec = Vec3(np.random.rand(3))
# Creating random rotation
q = Quaternion.from_euler(Vec3(np.random.rand(3) * 2 * math.pi))
# Rotate the vector by the rotation
rotated_vec = vec.rotate(q)
# transform the original vector to the NED coordinate system
vec_NED = Vec3.from_nwu(vec)
# transform the original quaternion to the NED coordinate system
q_NED = Quaternion.from_nwu(q)
# Rotate the converted vector by the converted rotation
rotated_vec_NED = vec_NED.rotate(q_NED)
# transform the rotated vector to the NED coordinate system
rotated_vec_NED_transformed = Vec3.from_nwu(rotated_vec)
np.testing.assert_allclose(rotated_vec_NED.numpy(),
rotated_vec_NED_transformed.numpy())
def getBaseVelocity(objectUniqueId: int) -> Tuple[Vec3, Vec3]:
"""
Gets the velocity and angular velocity of an object.
:param objectUniqueId: Id of the object.
:return: Tuple with velocity and angular velocity.
"""
linearVelocity, angularVelocity = p.getBaseVelocity(objectUniqueId)
return Vec3.from_nwu(linearVelocity), Vec3.from_nwu(angularVelocity)
def rayTest(rayFromPosition: Vec3, rayToPosition: Vec3, object_id=-1) -> Tuple[float, Vec3, Vec3]:
if object_id != -1:
rayFromPosition = translation.vec3_local_to_world_id(object_id, rayFromPosition)
rayToPosition = translation.vec3_local_to_world_id(object_id, rayToPosition)
_, _, hit_fraction, hit_position, hit_normal = p.rayTest(rayFromPosition.as_nwu(), rayToPosition.as_nwu())[0]
return hit_fraction, Vec3.from_nwu(hit_position), Vec3.from_nwu(hit_normal)
def getBasePositionAndOrientation(objectUniqueId: int) -> Tuple[Vec3, Quaternion]:
position, orientation = p.getBasePositionAndOrientation(objectUniqueId)
return Vec3.from_nwu(position), Quaternion.from_nwu(orientation)