Math Helpers

bosdyn.client.math_helpers.angle_diff(a1, a2)

bosdyn.client.math_helpers.angle_diff_degrees(a1, a2)

class bosdyn.client.math_helpers.SE2Pose(x, y, angle)

Bases: object

Class representing an SE2Pose with position and angle.

static flatten(se3pose)

Flatten a given SE3Pose to an SE2Pose. This will loose height information if the se3pose provided is not gravity aligned. The common gravity aligned frames are odom, vision, and flat_body.

Inputs:

se3pose (math_helpers.SE3Pose)

Returns

math_helpers.SE2Pose representing the flattened se3pose.

to_obj(proto)

Adds the math_helpers.SE2Pose properties into the geometry_pb2.SE2Pose ‘proto’.

to_proto()

Converts the math_helpers.SE2Pose into an output of the protobuf geometry_pb2.SE2Pose.

inverse()

Compute the inverse of the math_helpers.SE2Pose.

For example, if the SE(2) pose represented a_tform_b, then the inverse pose is b_tform_a.

Returns

math_helpers.SE2Pose representing the inverse of the current SE(2) Pose.

mult(se2pose)

Computes the multiplication between the current math_helpers.SE2Pose and the input se2pose.

For example, if the ‘self’ SE2Pose represents a_tform_b and the input se2pose represents b_tform_c, then the output will represent the transform a_tform_c.

Inputs:

se2pose (math_helpers.se2pose)

Returns

math_helpers.se2pose representing the multiplication of two SE(2) poses.

to_rot_matrix()

Returns the rotation matrix generate from the angle of the current SE(2) Pose.

to_matrix()

Returns the 3x3 matrix to transform a 2D point (in generalized coordinates).

to_adjoint_matrix()

This creates the adjoint matrix for the current SE2Pose.

The adjoint matrix can be used to change reference frames for a SE(2) velocity vector. For example, if you have math_helpers.SE2Velocity velocity_in_frame_b, then the adjoint matrix for the math_helpers.SE2Pose (representing a_tform_b) can be used as follows to transform the velocity: velocity_in_frame_a = a_tform_b.to_adjoint_matrix() * velocity_in_frame_b

Returns

a_adjoint_b (Numpy 3x3 matrix) representing the adjoint matrix for the SE2Pose.

property position

Property to allow attribute access of the protobuf message field ‘position’ similar to the geometry_pb2.SE2Pose for the math_helper.SE2Pose.

static from_matrix(mat)

Extract SE2Pose from a 3x3 matrix

static from_obj(tform)

Create a math_helpers.SE2Pose from a geometry_pb2.SE2Pose proto.

get_closest_se3_transform(height_z=0.0)

Compute the closest math_helpers.SE3Pose from the current math_helpers.SE2Pose.

class bosdyn.client.math_helpers.SE2Velocity(x, y, angular)

Bases: object

Class representing an SE2Velocity with linear velocity and angular velocity.

to_obj(proto)

Adds the math_helpers.SE2Velocity properties into the geometry_pb2.SE2Velocity ‘proto’.

to_proto()

Converts the math_helpers.SE2Velocity into an output of the protobuf geometry_pb2.SE2Velocity.

to_vector()

Creates a 3x1 velocity vector as a numpy array.

static from_vector(se2_vel_vector)

Converts a 3x1 velocity vector (of either a numpy array or a list) into a math_helpers.SE2Velocity object.

property linear

Property to allow attribute access of the protobuf message field ‘linear’ similar to the geometry_pb2.SE2Velocity for the math_helper.SE2Velocity.

property angular

Property to allow attribute access of the protobuf message field ‘angular’ similar to the geometry_pb2.SE2Velocity for the math_helper.SE2Velocity.

static from_obj(vel)

Create a math_helpers.SE2Velocity from a geometry_pb2.SE2Velocity proto.

class bosdyn.client.math_helpers.SE3Velocity(lin_x, lin_y, lin_z, ang_x, ang_y, ang_z)

Bases: object

Class representing an SE3Velocity with linear velocity and angular velocity.

to_obj(proto)

Adds the math_helpers.SE3Velocity properties into the geometry_pb2.SE3Velocity ‘proto’.

to_proto()

Converts the math_helpers.SE3Velocity into an output of the protobuf geometry_pb2.SE3Velocity.

to_vector()

Creates a 6x1 velocity vector as a numpy array.

property linear

Property to allow attribute access of the protobuf message field ‘linear’ similar to the geometry_pb2.SE3Velocity for the math_helper.SE3Velocity.

property angular

Property to allow attribute access of the protobuf message field ‘angular’ similar to the geometry_pb2.SE3Velocity for the math_helper.SE3Velocity.

static from_obj(vel)

Create a math_helpers.SE3Velocity from a geometry_pb2.SE3Velocity proto.

static from_vector(se3_vel_vector)

Converts a 6x1 velocity vector (of either a numpy array or a list) into a math_helpers.SE3Velocity object.

class bosdyn.client.math_helpers.SE3Pose(x, y, z, rot)

Bases: object

Class representing an SE3Pose with position and rotation.

static from_obj(tform)

Create a math_helpers.SE3Pose from a geometry_pb2.SE3Pose proto.

static from_se2(tform, z=0)

to_obj(proto)

Adds the math_helpers.SE3Pose properties into the geometry_pb2.SE3Pose ‘proto’.

to_proto()

Converts the math_helpers.SE3Pose into an output of the protobuf geometry_pb2.SE3Pose.

inverse()

Compute the inverse of the math_helpers.SE3Pose.

For example, if the SE(3) pose represented a_tform_b, then the inverse pose is b_tform_a.

Returns

math_helpers.SE3Pose representing the inverse of the current SE(3) Pose.

transform_point(x, y, z)

Compute the transformation (translation and rotation) of a (x,y,z) vector using the current SE(3) pose.

Returns

tuple (size 3) representing the transformed coordinate.

transform_cloud(points)

Compute the transformation (translation and rotation) of multiple vector/points using the current math_helpers.SE3Pose.

Inputs:

points (Nx3 numpy matrix) representing a set of (x,y,z) points to be transfromed

Returns

Nx3 numpy matrix of the points after they are transformed with the current math_helpers.SE3Pose.

static transform_cloud_from_matrix(transform, points)

Compute the transformation (translation and rotation) of multiple vector/points using the input SE(3) pose.

Inputs:

transform (4x4 numpy matrix) representing the SE3Pose to be used to transform. points (Nx3 numpy matrix) representing a set of (x,y,z) points to be transfromed

Returns

Nx3 numpy matrix of the points after they are transformed.

to_matrix()

Returns the 4x4 matrix to transform a 3D point (in generalized coordinates).

mult(se3pose)

Computes the multiplication between the current math_helpers.SE3Pose and the input se3pose.

For example, if the ‘self’ SE3Pose represents a_tform_b and the input se3pose represents b_tform_c, then the output will represent the transform a_tform_c.

Inputs:

se3pose (math_helpers.SE3Pose)

Returns

math_helpers.SE3Pose representing the multiplication of two SE(3) poses.

property position

Property to allow attribute access of the protobuf message field ‘position’ similar to the geometry_pb2.SE3Pose for the math_helper.SE3Pose.

property rotation

Property to allow attribute access of the protobuf message field ‘rotation’ similar to the geometry_pb2.SE3Pose for the math_helper.SE3Pose.

static from_matrix(mat)

Extract math_helpers.SE3Pose from a 4x4 matrix.

static from_identity()

Create a math_helpers.SE3Pose representing the identity SE(3) pose.

get_translation()

Returns a 3x1 numpy array representing the translation only of the current SE3Pose.

to_adjoint_matrix()

This creates the adjoint matrix for the current SE3Pose.

The adjoint matrix can be used to change reference frames for a SE(3) velocity vector. For example, if you have math_helpers.SE3Velocity velocity_in_frame_b, then the adjoint matrix for the math_helpers.SE3Pose (representing a_tform_b) can be used as follows to transform the velocity: velocity_in_frame_a = a_tform_b.to_adjoint_matrix() * velocity_in_frame_b

Returns

a_adjoint_b (Numpy 6x6 matrix) representing the adjoint matrix for the SE3Pose.

get_closest_se2_transform()

Compute the closest math_helpers.SE2Pose from the current math_helpers.SE3Pose.

class bosdyn.client.math_helpers.Quat(w=1, x=0, y=0, z=0)

Bases: object

Class representing a Quaternion.

inverse()

Computes the inverse of the current math_helpers.Quat.

transform_point(x, y, z)

“Computes the transformation (rotation by the quaternion) of a signle (x,y,z) point using the current math_helpers.Quat.

to_matrix()

Creates the 3x3 numpy rotation matrix from the current math_helpers.Quat

static from_matrix(rot)

Creates a math_helpers.Quat from a numpy 3x3 matrix representing rotation.

static from_roll(angle)

Computes a representative math_helpers.Quat from the Euler angle for roll.

static from_pitch(angle)

Computes a representative math_helpers.Quat from the Euler angle for pitch.

static from_yaw(angle)

Computes a representative math_helpers.Quat from the Euler angle for yaw.

to_roll()

Computes the Euler angle roll from the current math_helpers.Quat

to_pitch()

Computes the Euler angle pitch from the current math_helpers.Quat

to_yaw()

Computes the Euler angle yaw from the current math_helpers.Quat

to_axis_angle()

Computes the angle and the respective axis from the math_helpers.Quat

static from_obj(proto)

Create a math_helpers.Quat from a geometry_pb2.Quaternion proto.

to_obj(proto)

Adds the math_helpers.Quat properties into the geometry_pb2.Quaternion ‘proto’.

to_proto()

Converts the math_helpers.Quat into an output of the protobuf geometry_pb2.Quaternion.

mult(other_quat)

Computes the multiplication of two math_helpers.Quat’s.

normalize()

Normalizes the quaternion.

closest_yaw_only_quaternion()

Computes a yaw-only math_helpers.Quat from the current roll/pitch/yaw math_helpers.Quat

bosdyn.client.math_helpers.pose_to_xyz_yaw(A_tform_B)

Gets the x,y,z yaw of B in A from the SE3Pose protobuf message.

bosdyn.client.math_helpers.is_within_threshold(pose_3d, max_translation_meters, max_yaw_degrees)

Determines whether the given SE3 pose is small enough in X, Y, and theta.

bosdyn.client.math_helpers.recenter_angle(q, lower_limit, upper_limit)

bosdyn.client.math_helpers.skew_matrix_3d(vec3_proto)

Creates a 3x3 numpy matrix representing the skew symmetric matrix for a vec3. These are used to create the adjoint matrices for SE3Pose’s, among other things.

bosdyn.client.math_helpers.skew_matrix_2d(vec2_proto)

Creates a 2x1 numpy matrix representing the skew symmetric matrix for a vec2. These are used to create the adjoint matrices for SE2Pose’s, among other things.

bosdyn.client.math_helpers.transform_se2velocity(a_adjoint_b_matrix, se2_velocity_in_b)

Changes the frame that the SE(2) Velocity is expressed in. More specifcially, it converts the SE(2) Velocity in frame b to a SE(2) Velocity in frame c using the adjoint matrix a_adjoint_b.

Inputs:

a_adjoint_b_matrix (3x3 numpy matrix) se2_velocity_in_b (geometry_pb2.SE2Velocity OR math_helpers.SE2Velocity) described in frame B.

Returns

math_helpers.SE2Velocity described in frame a. None if the input velocity is an unknown type.

bosdyn.client.math_helpers.transform_se3velocity(a_adjoint_b_matrix, se3_velocity_in_b)

Changes the frame that the SE(3) Velocity is expressed in. More specifcially, it converts the SE(3) Velocity in frame b to a SE(3) Velocity in frame c using the adjoint matrix a_adjoint_b.

Inputs:

a_adjoint_b_matrix (6x6 numpy matrix) se3_velocity_in_b (geometry_pb2.SE3Velocity OR math_helpers.SE3Velocity) described in frame B.

Returns

math_helpers.SE3Velocity described in frame a. None if the input velocity is an unknown type.

bosdyn.client.math_helpers.quat_to_eulerZYX(q)

Convert a Quat object into Euler yaw, pitch, roll angles (radians).