# Math Helpers¶

`bosdyn.client.math_helpers.``angle_diff`(a1, a2)[source]
`bosdyn.client.math_helpers.``angle_diff_degrees`(a1, a2)[source]
class `bosdyn.client.math_helpers.``SE2Pose`(x, y, angle)[source]

Bases: `object`

Class representing an SE2Pose with position and angle.

static `flatten`(se3pose)[source]

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)[source]

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

`to_proto`()[source]

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

`inverse`()[source]

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)[source]

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`()[source]

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

`to_matrix`()[source]

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

`to_adjoint_matrix`()[source]

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

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)[source]

Extract SE2Pose from a 3x3 matrix

static `from_obj`(tform)[source]

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

`get_closest_se3_transform`(height_z=0.0)[source]

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

class `bosdyn.client.math_helpers.``SE2Velocity`(x, y, angular)[source]

Bases: `object`

Class representing an SE2Velocity with linear velocity and angular velocity.

`to_obj`(proto)[source]

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

`to_proto`()[source]

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

`to_vector`()[source]

Creates a 3x1 velocity vector as a numpy array.

static `from_vector`(se2_vel_vector)[source]

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)[source]

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)[source]

Bases: `object`

Class representing an SE3Velocity with linear velocity and angular velocity.

`to_obj`(proto)[source]

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

`to_proto`()[source]

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

`to_vector`()[source]

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)[source]

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

static `from_vector`(se3_vel_vector)[source]

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)[source]

Bases: `object`

Class representing an SE3Pose with position and rotation.

static `from_obj`(tform)[source]

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

static `from_se2`(tform, z=0)[source]
`to_obj`(proto)[source]

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

`to_proto`()[source]

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

`inverse`()[source]

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)[source]

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)[source]

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)[source]

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`()[source]

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

`mult`(se3pose)[source]

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)[source]

Extract math_helpers.SE3Pose from a 4x4 matrix.

static `from_identity`()[source]

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

`get_translation`()[source]

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

`to_adjoint_matrix`()[source]

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

`get_closest_se2_transform`()[source]

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)[source]

Bases: `object`

Class representing a Quaternion.

`inverse`()[source]

Computes the inverse of the current math_helpers.Quat.

`transform_point`(x, y, z)[source]

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

`to_matrix`()[source]

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

static `from_matrix`(rot)[source]

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

static `from_roll`(angle)[source]

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

static `from_pitch`(angle)[source]

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

static `from_yaw`(angle)[source]

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

`to_roll`()[source]

Computes the Euler angle roll from the current math_helpers.Quat

`to_pitch`()[source]

Computes the Euler angle pitch from the current math_helpers.Quat

`to_yaw`()[source]

Computes the Euler angle yaw from the current math_helpers.Quat

`to_axis_angle`()[source]

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

static `from_obj`(proto)[source]

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

`to_obj`(proto)[source]

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

`to_proto`()[source]

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

`mult`(other_quat)[source]

Computes the multiplication of two math_helpers.Quat’s.

`normalize`()[source]

Normalizes the quaternion.

`closest_yaw_only_quaternion`()[source]

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)[source]

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)[source]

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)[source]
`bosdyn.client.math_helpers.``skew_matrix_3d`(vec3_proto)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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