def to_proto(self):
"""Converts the math_helpers.SE3Velocity into an output of the protobuf geometry_pb2.SE3Velocity."""
return geometry_pb2.SE3Velocity(
linear=geometry_pb2.Vec3(x=self.linear_velocity_x, y=self.linear_velocity_y,
z=self.linear_velocity_z),
angular=geometry_pb2.Vec3(x=self.angular_velocity_x, y=self.angular_velocity_y,
z=self.angular_velocity_z))
def test_create_se3_vel():
# Test creating an SE3Velocity from a proto with from_obj()
proto_se3 = geometry_pb2.SE3Velocity(linear=geometry_pb2.Vec3(x=1,
y=2,
z=3),
angular=geometry_pb2.Vec3(x=1,
y=2,
z=3))
se3 = SE3Velocity.from_obj(proto_se3)
assert type(se3) == SE3Velocity
assert se3.linear_velocity_x == proto_se3.linear.x
assert se3.linear_velocity_y == proto_se3.linear.y
assert se3.linear_velocity_z == proto_se3.linear.z
assert se3.angular_velocity_x == proto_se3.angular.x
assert se3.angular_velocity_y == proto_se3.angular.y
assert se3.angular_velocity_z == proto_se3.angular.z
# Test proto-like attribute access properties
lin = se3.linear
assert type(lin) == geometry_pb2.Vec3
assert lin.x == proto_se3.linear.x
assert lin.y == proto_se3.linear.y
assert lin.z == proto_se3.linear.z
ang = se3.angular
assert type(ang) == geometry_pb2.Vec3
assert ang.x == proto_se3.angular.x
assert ang.y == proto_se3.angular.y
assert ang.z == proto_se3.angular.z
# Test going back to a proto message with to_proto()
new_proto_se3 = se3.to_proto()
assert type(new_proto_se3) == geometry_pb2.SE3Velocity
assert new_proto_se3.linear.x == proto_se3.linear.x
assert new_proto_se3.linear.y == proto_se3.linear.y
assert new_proto_se3.linear.z == proto_se3.linear.z
assert new_proto_se3.angular.x == proto_se3.angular.x
assert new_proto_se3.angular.y == proto_se3.angular.y
assert new_proto_se3.angular.z == proto_se3.angular.z
# Test mutating an existing proto message to_obj()
proto_mut_se3 = geometry_pb2.SE3Velocity()
se3.to_obj(proto_mut_se3)
assert type(proto_mut_se3) == geometry_pb2.SE3Velocity
assert proto_mut_se3.linear.x == proto_se3.linear.x
assert proto_mut_se3.linear.y == proto_se3.linear.y
assert proto_mut_se3.linear.z == proto_se3.linear.z
assert proto_mut_se3.angular.x == proto_se3.angular.x
assert proto_mut_se3.angular.y == proto_se3.angular.y
assert proto_mut_se3.angular.z == proto_se3.angular.z
# Test creating the velocity vector
vec = se3.to_vector()
assert type(vec) == numpy.ndarray
assert vec[0] == proto_se3.linear.x
assert vec[1] == proto_se3.linear.y
assert vec[2] == proto_se3.linear.z
assert vec[3] == proto_se3.angular.x
assert vec[4] == proto_se3.angular.y
assert vec[5] == proto_se3.angular.z
# Test creating the SE3Velocity from a array
vel_arr = numpy.array([1, 2, 3, 4, 5, 6]).reshape((6, 1))
se3 = SE3Velocity.from_vector(vel_arr)
assert type(se3) == SE3Velocity
assert se3.linear_velocity_x == 1
assert se3.linear_velocity_y == 2
assert se3.linear_velocity_z == 3
assert se3.angular_velocity_x == 4
assert se3.angular_velocity_y == 5
assert se3.angular_velocity_z == 6
# Test creating the SE2Velocity from a list
vel_list = [1, 2, 3, 4, 5, 6]
se3 = SE3Velocity.from_vector(vel_list)
assert type(se3) == SE3Velocity
assert se3.linear_velocity_x == 1
assert se3.linear_velocity_y == 2
assert se3.linear_velocity_z == 3
assert se3.angular_velocity_x == 4
assert se3.angular_velocity_y == 5
assert se3.angular_velocity_z == 6
